00001 #ifndef STG_H
00002 #define STG_H
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00049 #include <stdlib.h>
00050 #include <stdio.h>
00051 #include <sys/socket.h>
00052 #include <netdb.h>
00053 #include <string.h>
00054 #include <unistd.h>
00055 #include <stdint.h>
00056 #include <sys/types.h>
00057 #include <sys/time.h>
00058 #include <assert.h>
00059 #include <pthread.h>
00060 #include <semaphore.h>
00061
00062 #include <glib.h>
00063 #include <rtk.h>
00064
00065 #ifdef __cplusplus
00066 extern "C" {
00067 #endif
00068
00069 #include "config.h"
00070 #include "replace.h"
00071
00072
00078
00079 #define FiducialNone 0
00080
00083 #define RangecomNone 0
00084 #define RangecomImpede 1
00085 #define RangecomPoint 2
00086
00087
00088
00089
00090
00091 typedef int stg_id_t;
00092 typedef double stg_meters_t;
00093 typedef double stg_radians_t;
00094 typedef unsigned long stg_msec_t;
00095 typedef double stg_kg_t;
00096 typedef double stg_joules_t;
00097 typedef double stg_watts_t;
00098 typedef int stg_bool_t;
00099 typedef double stg_friction_t;
00100 typedef uint32_t stg_color_t;
00101 typedef int stg_obstacle_return_t;
00102 typedef int stg_blob_return_t;
00103 typedef int stg_fiducial_return_t;
00104 typedef int stg_rangecom_return_t;
00105 typedef int stg_ranger_return_t;
00106
00107 typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t;
00108
00111 typedef struct
00112 {
00113 stg_meters_t x, y;
00114 } stg_size_t;
00115
00118 typedef struct
00119 {
00120 stg_meters_t x, y, a;
00121 } stg_pose_t;
00122
00125 typedef stg_pose_t stg_velocity_t;
00126
00129 typedef struct
00130 {
00131 stg_pose_t pose;
00132 stg_size_t size;
00133 } stg_geom_t;
00134
00135
00136
00138 typedef struct
00139 {
00141 stg_joules_t stored;
00142
00144 stg_joules_t capacity;
00145
00147 stg_joules_t input_joules;
00148
00150 stg_joules_t output_joules;
00151
00153 stg_watts_t input_watts;
00154
00156 stg_watts_t output_watts;
00157
00159 stg_bool_t charging;
00160
00162 stg_meters_t range;
00163 } stg_energy_data_t;
00164
00166 typedef struct
00167 {
00169 stg_joules_t capacity;
00170
00172 stg_watts_t give;
00173
00176 stg_watts_t take;
00177
00179 stg_meters_t probe_range;
00180 } stg_energy_config_t;
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204 #define STG_MOVE_TRANS (1 << 0)
00205 #define STG_MOVE_ROT (1 << 1)
00206 #define STG_MOVE_SCALE (1 << 2)
00207
00208 typedef int stg_movemask_t;
00209
00210 typedef struct
00211 {
00212 uint8_t show_data;
00213 uint8_t show_cfg;
00214 uint8_t show_cmd;
00215
00216 uint8_t nose;
00217 uint8_t grid;
00218
00219 uint8_t outline;
00220 stg_movemask_t movemask;
00221 } stg_guifeatures_t;
00222
00223
00224
00225
00227 typedef enum
00228 {
00229 LaserTransparent,
00230 LaserVisible,
00231 LaserBright
00232 } stg_laser_return_t;
00233
00237
00238 stg_msec_t stg_timenow( void );
00239
00243 int stg_init( int argc, char** argv );
00244
00248 const char* stg_get_version_string( void );
00249
00251 int stg_quit_test( void );
00252
00254 void stg_quit_request( void );
00255
00257 void stg_err( const char* err );
00258
00259
00260
00261
00266 void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 );
00267
00268
00269
00274 typedef struct
00275 {
00276 stg_pose_t pose;
00277 stg_size_t size;
00278 } stg_rotrect_t;
00279
00283 void stg_normalize_rects( stg_rotrect_t* rects, int num );
00284
00289 int stg_load_image( const char* filename,
00290 stg_rotrect_t** rects,
00291 int* rect_count,
00292 int* widthp, int* heightp );
00293
00298 void stg_print_geom( stg_geom_t* geom );
00299
00304 stg_color_t stg_lookup_color(const char *name);
00305
00306
00307
00313 typedef struct
00314 {
00315 stg_meters_t x, y;
00316 } stg_point_t;
00317
00319 stg_point_t* stg_points_create( size_t count );
00320
00322 stg_point_t* stg_point_create( void );
00323
00325 void stg_points_destroy( stg_point_t* pts );
00326
00328 void stg_point_destroy( stg_point_t* pt );
00329
00332
00333
00339 typedef struct
00340 {
00342 GArray* points;
00343
00345 stg_bool_t filled;
00346
00348 stg_color_t color;
00349 } stg_polygon_t;
00350
00351
00353 stg_polygon_t* stg_polygons_create( int count );
00354
00356 void stg_polygons_destroy( stg_polygon_t* p, size_t count );
00357
00359 stg_polygon_t* stg_polygon_create( void );
00360
00363 stg_polygon_t* stg_unit_polygon_create( void );
00364
00371
00372 void stg_polygon_destroy( stg_polygon_t* p );
00373
00377 void stg_polygon_set_points( stg_polygon_t* poly, stg_point_t* pts, size_t count );
00380 void stg_polygon_append_points( stg_polygon_t* poly, stg_point_t* pts, size_t count );
00381
00382 stg_polygon_t* stg_rects_to_polygons( stg_rotrect_t* rects, size_t count );
00383
00386 void stg_normalize_polygons( stg_polygon_t* polys, int num,
00387 double width, double height );
00388
00390 void stg_polygon_print( stg_polygon_t* poly );
00391
00393 void stg_polygons_print( stg_polygon_t* polys, unsigned int count );
00394
00397
00401
00402
00403
00404
00405
00406
00410 void stg_get_default_pose( stg_pose_t* pose );
00411 void stg_get_default_geom( stg_geom_t* geom );
00412
00415
00416
00417
00418
00419 struct _stg_world;
00420 struct _stg_model;
00421 struct _stg_matrix;
00422 struct _gui_window;
00423
00429 typedef struct _stg_model stg_model_t;
00430
00433
00434
00442 typedef struct _stg_world stg_world_t;
00443
00444
00447 stg_world_t* stg_world_create( stg_id_t id,
00448 const char* token,
00449 int sim_interval,
00450 int real_interval,
00451 double ppm,
00452 double width,
00453 double height );
00454
00457 stg_world_t* stg_world_create_from_file( const char* worldfile_path );
00458
00461 void stg_world_destroy( stg_world_t* world );
00462
00465 void stg_world_stop( stg_world_t* world );
00466
00469 void stg_world_start( stg_world_t* world );
00470
00473 int stg_world_update( stg_world_t* world, int sleepflag );
00474
00476 void stg_world_load( stg_world_t* mod );
00477
00479 void stg_world_save( stg_world_t* mod );
00480
00483 void stg_world_print( stg_world_t* world );
00484
00487 void stg_world_set_interval_real( stg_world_t* world, unsigned int val );
00488
00492 void stg_world_set_interval_sim( stg_world_t* world, unsigned int val );
00493
00495 stg_model_t* stg_world_get_model( stg_world_t* world, stg_id_t mid );
00496
00498 stg_model_t* stg_world_model_name_lookup( stg_world_t* world, const char* name );
00499
00500
00501 struct stg_property;
00502
00503 typedef int (*stg_property_callback_t)(stg_model_t* mod, char* name, void* data, size_t len, void* userdata );
00504
00505 typedef void (*stg_property_storage_func_t)( struct stg_property* prop,
00506 void* data, size_t len );
00507
00508
00510 void stg_world_add_property_callback( stg_world_t* world,
00511 char* propname,
00512 stg_property_callback_t callback,
00513 void* userdata );
00514
00516 void stg_world_remove_property_callback( stg_world_t* world,
00517 char* propname,
00518 stg_property_callback_t callback );
00519
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531 void stg_model_add_property_toggles( stg_model_t* mod,
00532 const char* propname,
00533 stg_property_callback_t callback_on,
00534 void* arg_on,
00535 stg_property_callback_t callback_off,
00536 void* arg_off,
00537 const char* label,
00538 int enabled );
00539
00540 int stg_model_fig_clear_cb( stg_model_t* mod, void* data, size_t len,
00541 void* userp );
00546
00547
00548
00556 #define STG_PROPNAME_MAX 128
00557
00558 typedef struct
00559 {
00560 stg_property_callback_t callback;
00561 void* arg;
00562 } stg_cbarg_t;
00563
00564 typedef struct stg_property {
00565 char name[STG_PROPNAME_MAX];
00566 void* data;
00567 size_t len;
00568 stg_property_storage_func_t storage_func;
00569 GList* callbacks;
00570
00571 stg_model_t* mod;
00572
00573 } stg_property_t;
00574
00575
00576 typedef int(*stg_model_initializer_t)(stg_model_t*);
00577
00579 stg_model_t* stg_model_create( stg_world_t* world,
00580 stg_model_t* parent,
00581 stg_id_t id,
00582 char* token,
00583 stg_model_initializer_t initializer );
00584
00586 void stg_model_destroy( stg_model_t* mod );
00587
00589 void stg_model_get_global_pose( stg_model_t* mod, stg_pose_t* pose );
00590
00592 void stg_model_get_global_velocity( stg_model_t* mod, stg_velocity_t* gvel );
00593
00594
00595
00596
00598 void stg_model_subscribe( stg_model_t* mod );
00599
00601 void stg_model_unsubscribe( stg_model_t* mod );
00602
00604 void stg_model_load( stg_model_t* mod );
00605
00607 void stg_model_save( stg_model_t* mod );
00608
00610
00611
00612
00613
00615 int stg_model_set_global_pose( stg_model_t* mod, stg_pose_t* gpose );
00616
00618 int stg_model_set_velocity( stg_model_t* mod, stg_velocity_t* vel );
00619
00622 void stg_model_lock( stg_model_t* mod );
00623
00625 void stg_model_unlock( stg_model_t* mod );
00626
00628 int stg_model_set_parent( stg_model_t* mod, stg_model_t* newparent);
00629
00630 void stg_model_get_geom( stg_model_t* mod, stg_geom_t* dest );
00631 void stg_model_get_velocity( stg_model_t* mod, stg_velocity_t* dest );
00632
00633 stg_property_t* stg_model_set_property( stg_model_t* mod,
00634 const char* prop,
00635 void* data,
00636 size_t len );
00637
00638 stg_property_t* stg_model_set_property_ex( stg_model_t* mod,
00639 const char* prop,
00640 void* data,
00641 size_t len,
00642 stg_property_storage_func_t func );
00643
00646 void* stg_model_get_property( stg_model_t* mod,
00647 const char* prop,
00648 size_t* len );
00649
00652 void* stg_model_get_property_fixed( stg_model_t* mod,
00653 const char* name,
00654 size_t size );
00655
00656 void stg_model_property_refresh( stg_model_t* mod, const char* propname );
00657
00658
00661 stg_polygon_t* stg_model_get_polygons( stg_model_t* mod, size_t* poly_count );
00662 void stg_model_set_polygons( stg_model_t* mod,
00663 stg_polygon_t* polys,
00664 size_t poly_count );
00665
00666
00667
00668
00669
00670 int stg_model_add_property_callback( stg_model_t* mod, const char* prop,
00671 stg_property_callback_t, void* user );
00672
00673 int stg_model_remove_property_callback( stg_model_t* mod, const char* prop,
00674 stg_property_callback_t );
00675
00676 int stg_model_remove_property_callbacks( stg_model_t* mod, const char* prop );
00677
00680 void stg_model_print( stg_model_t* mod );
00681
00684 int stg_model_is_antecedent( stg_model_t* mod, stg_model_t* testmod );
00685
00688 int stg_model_is_descendent( stg_model_t* mod, stg_model_t* testmod );
00689
00693 int stg_model_is_related( stg_model_t* mod1, stg_model_t* mod2 );
00694
00696 stg_model_t* stg_model_root( stg_model_t* mod );
00697
00700 int stg_model_tree_to_ptr_array( stg_model_t* root, GPtrArray* array );
00701
00703 int stg_model_startup( stg_model_t* mod );
00704
00706 int stg_model_shutdown( stg_model_t* mod );
00707
00708
00709 int stg_model_update( stg_model_t* model );
00710
00712 void stg_model_global_to_local( stg_model_t* mod, stg_pose_t* pose );
00713 void stg_model_local_to_global( stg_model_t* mod, stg_pose_t* pose );
00714
00715
00719
00720
00725 #define STG_BLOB_CHANNELS_MAX 16
00726
00729 typedef struct
00730 {
00731 int channel_count;
00732 stg_color_t channels[STG_BLOB_CHANNELS_MAX];
00733 int scan_width;
00734 int scan_height;
00735 stg_meters_t range_max;
00736 stg_radians_t pan, tilt, zoom;
00737 } stg_blobfinder_config_t;
00738
00741 typedef struct
00742 {
00743 int channel;
00744 stg_color_t color;
00745 int xpos, ypos;
00746
00747 int left, top, right, bottom;
00748 int area;
00749 stg_meters_t range;
00750 } stg_blobfinder_blob_t;
00751
00754 stg_model_t* stg_blobfinder_create( stg_world_t* world,
00755 stg_model_t* parent,
00756 stg_id_t id,
00757 char* token );
00760
00761
00768 typedef struct
00769 {
00770 uint32_t range;
00771 char reflectance;
00772 } stg_laser_sample_t;
00773
00776 typedef struct
00777 {
00778
00779 stg_radians_t fov;
00780 stg_meters_t range_max;
00781 stg_meters_t range_min;
00782 int samples;
00783
00784 } stg_laser_config_t;
00785
00788 void stg_print_laser_config( stg_laser_config_t* slc );
00789
00792 stg_model_t* stg_laser_create( stg_world_t* world,
00793 stg_model_t* parent,
00794 stg_id_t id,
00795 char* token );
00796
00799 size_t stg_model_get_data_laser( stg_model_t* mod,
00800 stg_laser_sample_t* data,
00801 size_t max_samples );
00804
00805
00810 typedef enum {
00811 STG_GRIPPER_PADDLE_OPEN = 0,
00812 STG_GRIPPER_PADDLE_CLOSED,
00813 STG_GRIPPER_PADDLE_OPENING,
00814 STG_GRIPPER_PADDLE_CLOSING,
00815 } stg_gripper_paddle_state_t;
00816
00817 typedef enum {
00818 STG_GRIPPER_LIFT_DOWN = 0,
00819 STG_GRIPPER_LIFT_UP,
00820 STG_GRIPPER_LIFT_UPPING,
00821 STG_GRIPPER_LIFT_DOWNING,
00822 } stg_gripper_lift_state_t;
00823
00824 typedef enum {
00825 STG_GRIPPER_CMD_NOP = 0,
00826 STG_GRIPPER_CMD_OPEN,
00827 STG_GRIPPER_CMD_CLOSE,
00828 STG_GRIPPER_CMD_UP,
00829 STG_GRIPPER_CMD_DOWN
00830 } stg_gripper_cmd_type_t;
00831
00834 typedef struct
00835 {
00836 stg_size_t paddle_size;
00837
00838 stg_gripper_paddle_state_t paddles;
00839 stg_gripper_lift_state_t lift;
00840
00841 double paddle_position;
00842 double lift_position;
00843
00844 stg_meters_t inner_break_beam_inset;
00845 stg_meters_t outer_break_beam_inset;
00846 stg_bool_t paddles_stalled;
00847
00848
00849 GSList *grip_stack;
00850 int grip_stack_size;
00851
00852 } stg_gripper_config_t;
00853
00856 typedef struct
00857 {
00858 stg_gripper_cmd_type_t cmd;
00859 int arg;
00860 } stg_gripper_cmd_t;
00861
00862
00865 typedef struct
00866 {
00867 stg_gripper_paddle_state_t paddles;
00868 stg_gripper_lift_state_t lift;
00869
00870 double paddle_position;
00871 double lift_position;
00872
00873 stg_bool_t inner_break_beam;
00874 stg_bool_t outer_break_beam;
00875
00876 stg_bool_t left_paddle_contact[3];
00877 stg_bool_t right_paddle_contact[3];
00878
00879 stg_bool_t paddles_stalled;
00880
00881
00882 int stack_count;
00883
00884 } stg_gripper_data_t;
00885
00886
00889 void stg_print_gripper_config( stg_gripper_config_t* slc );
00890
00893 stg_model_t* stg_gripper_create( stg_world_t* world,
00894 stg_model_t* parent,
00895 stg_id_t id,
00896 char* token );
00899
00900
00907 typedef struct
00908 {
00909 stg_meters_t max_range_anon;
00910 stg_meters_t max_range_id;
00911 stg_meters_t min_range;
00912 stg_radians_t fov;
00913 stg_radians_t heading;
00914
00915 } stg_fiducial_config_t;
00916
00919 typedef struct
00920 {
00921 stg_meters_t range;
00922 stg_radians_t bearing;
00923 stg_pose_t geom;
00924 int id;
00925
00926 } stg_fiducial_t;
00927
00930 stg_model_t* stg_fiducial_create( stg_world_t* world,
00931 stg_model_t* parent,
00932 stg_id_t id,
00933 char* token );
00936
00937
00944 typedef struct
00945 {
00946 stg_meters_t max_range;
00947 stg_meters_t wall_penalty;
00948 } stg_rangecom_config_t;
00949
00953 typedef struct
00954 {
00955 stg_meters_t range;
00956 stg_meters_t bearing;
00957 char name[32];
00958 } stg_rangecom_t;
00959
00963 stg_model_t * stg_rangecom_create ( stg_world_t* world,
00964 stg_model_t* parent,
00965 stg_id_t id,
00966 char *token );
00969
00976 typedef struct
00977 {
00978 double lat_base;
00979 double lon_base;
00980 } stg_gps_config_t;
00981
00985 typedef struct
00986 {
00987 double latitude;
00988 double longitude;
00989 double sec;
00990 double usec;
00991 } stg_gps_t;
00992
00996 stg_model_t * stg_gps_create ( stg_world_t* world,
00997 stg_model_t* parent,
00998 stg_id_t id,
00999 char *token );
01002
01008 typedef struct
01009 {
01010 double north;
01011 } stg_compass_config_t;
01012
01013 typedef struct
01014 {
01015 double to_north;
01016 } stg_compass_t;
01017
01020
01021
01027 typedef struct
01028 {
01029 stg_meters_t min, max;
01030 } stg_bounds_t;
01031
01032 typedef struct
01033 {
01034 stg_bounds_t range;
01035 stg_radians_t angle;
01036 } stg_fov_t;
01037
01038 typedef struct
01039 {
01040 stg_pose_t pose;
01041 stg_size_t size;
01042 stg_bounds_t bounds_range;
01043 stg_radians_t fov;
01044 } stg_ranger_config_t;
01045
01046 typedef struct
01047 {
01048 stg_meters_t range;
01049
01050 } stg_ranger_sample_t;
01051
01054 stg_model_t* stg_ranger_create( stg_world_t* world,
01055 stg_model_t* parent,
01056 stg_id_t id,
01057 char* token );
01060
01061
01066
01067
01068 typedef enum
01069 { STG_POSITION_CONTROL_VELOCITY, STG_POSITION_CONTROL_POSITION }
01070 stg_position_control_mode_t;
01071
01072 #define STG_POSITION_CONTROL_DEFAULT STG_POSITION_CONTROL_VELOCITY
01073
01074 typedef enum
01075 { STG_POSITION_LOCALIZATION_GPS, STG_POSITION_LOCALIZATION_ODOM }
01076 stg_position_localization_mode_t;
01077
01078 #define STG_POSITION_LOCALIZATION_DEFAULT STG_POSITION_LOCALIZATION_GPS
01079
01081 typedef enum
01082 { STG_POSITION_DRIVE_DIFFERENTIAL, STG_POSITION_DRIVE_OMNI }
01083 stg_position_drive_mode_t;
01084
01085 #define STG_POSITION_DRIVE_DEFAULT STG_POSITION_DRIVE_DIFFERENTIAL
01086
01088 typedef struct
01089 {
01090 stg_meters_t x,y,a;
01091 stg_position_control_mode_t mode;
01092 } stg_position_cmd_t;
01093
01095 typedef struct
01096 {
01097 stg_pose_t pose;
01098 stg_pose_t pose_error;
01099 stg_pose_t origin;
01100 stg_velocity_t velocity;
01101 stg_velocity_t integration_error;
01102 stg_bool_t stall;
01103 stg_position_localization_mode_t localization;
01104 } stg_position_data_t;
01105
01107 typedef int stg_position_stall_t;
01108
01110 stg_model_t* stg_position_create( stg_world_t* world, stg_model_t* parent, stg_id_t id, char* token );
01111
01113 void stg_model_position_set_odom( stg_model_t* mod, stg_pose_t* odom );
01114
01117
01122
01123
01124
01125
01126 #ifndef TRUE
01127 #define TRUE 1
01128 #endif
01129
01130 #ifndef FALSE
01131 #define FALSE 0
01132 #endif
01133
01134 #define MILLION 1e6
01135 #define BILLION 1e9
01136
01137 #ifndef M_PI
01138 #define M_PI 3.14159265358979323846
01139 #endif
01140
01141 #ifndef TWOPI
01142 #define TWOPI (2.0*M_PI)
01143 #endif
01144
01145 #ifndef RTOD
01146
01147 #define RTOD(r) ((r) * 180.0 / M_PI)
01148 #endif
01149
01150 #ifndef DTOR
01151
01152 #define DTOR(d) ((d) * M_PI / 180.0)
01153 #endif
01154
01155 #ifndef NORMALIZE
01156
01157 #define NORMALIZE(z) atan2(sin(z), cos(z))
01158 #endif
01159
01160
01161 #ifdef __cplusplus
01162 }
01163 #endif
01164
01165
01168 #endif