Stage

• Frontpage
• Release
• Contents

User

• Introduction
• Installation
• Help
• FAQ

  ---

• World
• Window
• Model
  • blobfinder
  • bumper
  • fiducial
  • gripper
  • laser
  • position
  • ptz
  • ranger
  • speech

  ---

• Player plugin

Developer

libstage Index

Online

Homepage
Download
Project
Bugs
Help

stage.h

Go to the documentation of this file.

#ifndef STG_H
#define STG_H
/*
 *  Stage : a multi-robot simulator.  
 * 
 *  Copyright (C) 2001-2004 Richard Vaughan, Andrew Howard and Brian
 *  Gerkey for the Player/Stage Project
 *  http://playerstage.sourceforge.net
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* File: stage.h
 * Desc: External header file for the Stage library
 * Authors: Richard Vaughan vaughan@sfu.ca 
 *          Andrew Howard ahowards@usc.edu
 *          Brian Gerkey gerkey@stanford.edu
 * Date: 1 June 2003
 * CVS: $Id: stage.h,v 1.193 2007/09/11 21:30:09 rtv Exp $
 */


#include <unistd.h>
#include <stdint.h> // for portable int types eg. uint32_t
#include <sys/types.h>
#include <sys/time.h>
#include <glib.h> // we use GLib's data structures extensively

#ifdef __cplusplus
extern "C" {
#endif 

#define FiducialNone 0

#define STG_TOKEN_MAX 64

  typedef int stg_id_t;
  
  typedef double stg_meters_t;
  
  typedef double stg_radians_t;
  
  typedef unsigned long stg_msec_t;

  typedef double stg_kg_t; // Kilograms (mass)

  typedef double stg_joules_t;

  typedef double stg_watts_t; 

  typedef int stg_bool_t;

  //typedef double stg_friction_t;
  
  typedef uint32_t stg_color_t;

  //typedef int stg_obstacle_return_t;

  //typedef int stg_blob_return_t;

  //typedef int stg_fiducial_return_t;

  //typedef int stg_ranger_return_t;
  
  //typedef enum { STG_GRIP_NO = 0, STG_GRIP_YES } stg_gripper_return_t;
  
  typedef struct 
  {
    stg_meters_t x, y;
  } stg_size_t;
  
  typedef struct
  {
    stg_meters_t x, y, a;
  } stg_pose_t;
  
  typedef stg_pose_t stg_velocity_t;  

  typedef struct
  {
    stg_pose_t pose;
    stg_size_t size;
  } stg_geom_t;
  
  typedef struct
  {
    double min; //< smallest value in range
    double max; //< largest value in range
  } stg_bounds_t;
  
  typedef struct
  {
    stg_bounds_t range; //< min and max range of sensor
    stg_radians_t angle; //< width of viewing angle of sensor
  } stg_fov_t;

  typedef struct 
  {
    stg_radians_t pan;
    stg_radians_t tilt;
    stg_radians_t zoom;
  } stg_ptz_t;
 // end types


  // GUIFEATURES -------------------------------------------------------
  
  
 // forward declare struct types from player_internal.h
  struct _stg_model;
  struct _stg_matrix;
  struct _gui_window;
  struct _stg_world;

  typedef struct _stg_model stg_model_t; //  defined in stage_internal.
  typedef struct _stg_world stg_world_t; 



  stg_msec_t stg_timenow( void );
  
  int stg_init( int argc, char** argv );
  
  
  int stg_quit_test( void );

  void stg_quit_request( void );

  
  const char* stg_version_string( void );


  // POINTS ---------------------------------------------------------
  typedef struct
  {
    stg_meters_t x, y;
  } stg_point_t;

  stg_point_t* stg_points_create( size_t count );

  void stg_points_destroy( stg_point_t* pts );

  // POLYLINES ---------------------------------------------------------

  typedef struct
  {
    stg_point_t* points; 
    size_t points_count; 
  } stg_polyline_t; 

  // POLYGONS ---------------------------------------------------------
  
  typedef struct
  {
    GArray* points;
    
    stg_bool_t unfilled; 
    
    stg_color_t color;

    stg_size_t bbox;

    void* _data; // temporary internal use only
  } stg_polygon_t; 

  
  stg_polygon_t* stg_polygons_create( int count );
  
  void stg_polygons_destroy( stg_polygon_t* p, size_t count );
  
  stg_polygon_t* stg_unit_polygon_create( void );
    
  void stg_polygon_set_points( stg_polygon_t* poly, stg_point_t* pts, size_t count );                          
  void stg_polygon_append_points( stg_polygon_t* poly, stg_point_t* pts, size_t count );                               
  
  void stg_polygons_normalize( stg_polygon_t* polys, int num, 
                               double width, double height );
  
  void stg_polygon_print( stg_polygon_t* poly );
  
  void stg_polygons_print( stg_polygon_t* polys, unsigned int count );
  
  stg_polygon_t* stg_polygons_from_image_file(  const char* filename, 
                                                size_t* poly_count );
  
  // end property typedefs -------------------------------------------------


 //  WORLD --------------------------------------------------------

  stg_world_t* stg_world_create( stg_id_t id, 
                                 const char* token, 
                                 int sim_interval, 
                                 int real_interval,
                                 double ppm,
                                 double width,
                                 double height );

  stg_world_t* stg_world_create_from_file( const char* worldfile_path );

  void stg_world_destroy( stg_world_t* world );
  
  void stg_world_stop( stg_world_t* world );
  
  void stg_world_start( stg_world_t* world );

  int stg_world_update( stg_world_t* world, int sleepflag );

  void stg_world_load( stg_world_t* mod );

  void stg_world_save( stg_world_t* mod );

  void stg_world_print( stg_world_t* world );

  void stg_world_set_interval_real( stg_world_t* world, unsigned int val );
  
  void stg_world_set_interval_sim( stg_world_t* world, unsigned int val );

  stg_model_t* stg_world_get_model( stg_world_t* world, stg_id_t mid );
  
  stg_model_t* stg_world_model_name_lookup( stg_world_t* world, const char* name );
  
  //  MODEL --------------------------------------------------------
    
  // group the docs of all the model types
  // Movement masks for figures
#define STG_MOVE_TRANS (1 << 0)
#define STG_MOVE_ROT   (1 << 1)
#define STG_MOVE_SCALE (1 << 2)
  
  typedef int stg_movemask_t;

  typedef int (*stg_model_callback_t)(stg_model_t* mod, void* user );

  void stg_model_add_callback( stg_model_t* mod, 
                               void* member, 
                               stg_model_callback_t cb, 
                               void* user );

  int stg_model_remove_callback( stg_model_t* mod,
                                 void* member,
                                 stg_model_callback_t callback );

  
  typedef int(*stg_model_initializer_t)(stg_model_t*);
  

  
  typedef enum 
    {
      LaserTransparent, 
      LaserVisible, 
      LaserBright  
    } stg_laser_return_t;

  stg_model_t* stg_model_create(  stg_world_t* world,
                                  stg_model_t* parent, 
                                  stg_id_t id, 
                                  char* typestr );
  
  void stg_model_destroy( stg_model_t* mod );

  void stg_model_get_global_pose( stg_model_t* mod, stg_pose_t* pose );

  void stg_model_get_global_velocity( stg_model_t* mod, stg_velocity_t* gvel );

  /* set the velocity of a model in the global coordinate system */
  void stg_model_set_global_velocity( stg_model_t* mod, stg_velocity_t* gvel );

  void stg_model_subscribe( stg_model_t* mod );

  void stg_model_unsubscribe( stg_model_t* mod );

  void stg_model_load( stg_model_t* mod );

  void stg_model_save( stg_model_t* mod );

  //const char* stg_model_type_string( stg_model_type_t type );

  // SET properties - use these to set props, don't set them directly

  void stg_model_set_global_pose( stg_model_t* mod, stg_pose_t* gpose );
  
  void stg_model_set_velocity( stg_model_t* mod, stg_velocity_t* vel );
 
  void stg_model_set_pose( stg_model_t* mod, stg_pose_t* pose );

  void stg_model_set_geom( stg_model_t* mod, stg_geom_t* src );

  void stg_model_set_fiducial_return( stg_model_t* mod, int fid );

  void stg_model_set_fiducial_key( stg_model_t* mod, int key );

  int stg_model_set_parent( stg_model_t* mod, stg_model_t* newparent);
  
  void stg_model_get_geom( stg_model_t* mod, stg_geom_t* dest );

  void stg_model_get_pose( stg_model_t* mod, stg_pose_t* dest );

  void stg_model_get_velocity( stg_model_t* mod, stg_velocity_t* dest );
  
  stg_polygon_t* stg_model_get_polygons( stg_model_t* mod, size_t* poly_count );
  void stg_model_set_polygons( stg_model_t* mod,
                               stg_polygon_t* polys, 
                               size_t poly_count );
  
  void stg_model_set_lines( stg_model_t* mod,
                            stg_polyline_t* lines, 
                            size_t lines_count );
  
  // guess what these do?
  void stg_model_get_velocity( stg_model_t* mod, stg_velocity_t* dest );
  void stg_model_set_velocity( stg_model_t* mod, stg_velocity_t* vel );
  void stg_model_set_pose( stg_model_t* mod, stg_pose_t* pose );
  void stg_model_get_geom( stg_model_t* mod, stg_geom_t* dest );
  void stg_model_set_geom( stg_model_t* mod, stg_geom_t* geom );
  void stg_model_set_color( stg_model_t* mod, stg_color_t col );
  void stg_model_set_mass( stg_model_t* mod, stg_kg_t mass );
  void stg_model_set_stall( stg_model_t* mod, stg_bool_t stall );
  void stg_model_set_gripper_return( stg_model_t* mod, int val );
  void stg_model_set_audio_return( stg_model_t* mod, int val );
  void stg_model_set_laser_return( stg_model_t* mod, int val );
  void stg_model_set_obstacle_return( stg_model_t* mod, int val );
  void stg_model_set_blob_return( stg_model_t* mod, int val );
  void stg_model_set_ranger_return( stg_model_t* mod, int val );
  void stg_model_set_boundary( stg_model_t* mod, int val );
  void stg_model_set_gui_nose( stg_model_t* mod, int val );
  void stg_model_set_gui_mask( stg_model_t* mod, int val );
  void stg_model_set_gui_grid( stg_model_t* mod, int val );
  void stg_model_set_gui_outline( stg_model_t* mod, int val );
  void stg_model_set_watts( stg_model_t* mod, stg_watts_t watts );
  void stg_model_set_map_resolution( stg_model_t* mod, stg_meters_t res );

#define STG_MP_PREFIX          "_mp_"

#define STG_MP_POSE                     "_mp_pose"
#define STG_MP_VELOCITY                 "_mp_velocity"
#define STG_MP_GEOM                     "_mp_geom"
#define STG_MP_COLOR                    "_mp_color"
#define STG_MP_WATTS                    "_mp_watts"
#define STG_MP_FIDUCIAL_RETURN          "_mp_fiducial_return"
#define STG_MP_LASER_RETURN             "_mp_laser_return"
#define STG_MP_OBSTACLE_RETURN          "_mp_obstacle_return"
#define STG_MP_RANGER_RETURN            "_mp_ranger_return"
#define STG_MP_GRIPPER_RETURN           "_mp_gripper_return"
#define STG_MP_AUDIO_RETURN             "_mp_audio_return"
#define STG_MP_MASS                     "_mp_mass"
  
  /* TODO - complete the set of named properties */

  int stg_model_set_property( stg_model_t* mod, 
                              char* propname, 
                              void* data );
  
  void stg_model_unset_property( stg_model_t* mod,
                                 char* propname );

  void* stg_model_get_property( stg_model_t* mod, char* key );
  

/*   /\** @TODO The callback cb will be called with argument arg when the */
/*       property identified by key is set. *\/ */
/*   void stg_model_set_property_callback( stg_model_t* mod, */
/*                                      char* key, */
/*                                      stg_property_callback cb, */
/*                                      void* argn ); */
 
  void stg_model_print( stg_model_t* mod, char* prefix );
  
  int stg_model_is_antecedent( stg_model_t* mod, stg_model_t* testmod );
  
  int stg_model_is_descendent( stg_model_t* mod, stg_model_t* testmod );
  
  int stg_model_is_related( stg_model_t* mod1, stg_model_t* mod2 );

  stg_model_t* stg_model_root( stg_model_t* mod );
  
  GPtrArray* stg_model_array_from_tree( stg_model_t* root );
  
  int stg_model_startup( stg_model_t* mod );

  int stg_model_shutdown( stg_model_t* mod );

  int stg_model_update( stg_model_t* model );
  
  void stg_model_global_to_local( stg_model_t* mod, stg_pose_t* pose );
  
  void stg_model_local_to_global( stg_model_t* mod, stg_pose_t* pose );

  int stg_model_fig_clear_cb( stg_model_t* mod, void* data, size_t len, 
                              void* userp );

  void stg_model_set_data( stg_model_t* mod, void* data, size_t len );
  void stg_model_set_cmd( stg_model_t* mod, void* cmd, size_t len );
  void stg_model_set_cfg( stg_model_t* mod, void* cfg, size_t len );
  void* stg_model_get_cfg( stg_model_t* mod, size_t* lenp );
  void* stg_model_get_data( stg_model_t* mod, size_t* lenp );
  void* stg_model_get_cmd( stg_model_t* mod, size_t* lenp );
  
  void stg_model_draw_polygons( stg_model_t* mod, const char* group,
                                double x, double y, double a, 
                                stg_polygon_t* polys, size_t polycount );
  
  void stg_model_draw_polylines( stg_model_t* mod, const char* group, 
                                 double x, double y, double a, 
                                 stg_polyline_t* lines, size_t linecount,
                                 double thickness, stg_color_t colot );
  
  void stg_model_draw_points( stg_model_t* mod, const char* group,
                              double x, double y, double a, 
                              stg_point_t* points, size_t point, 
                              double size, stg_color_t color );
  

  void stg_model_add_property_toggles( stg_model_t* mod,
                                       void* member,
                                       stg_model_callback_t callback_on,
                                       void* arg_on,
                                       stg_model_callback_t callback_off,
                                       void* arg_off,
                                       const char* name,
                                       const char* label,
                                       gboolean enabled );


  // BLOBFINDER MODEL --------------------------------------------------------
  
#define STG_BLOB_CHANNELS_MAX 16
  
  typedef struct
  {
    int channel_count; // 0 to STG_BLOBFINDER_CHANNELS_MAX
    stg_color_t channels[STG_BLOB_CHANNELS_MAX];
    int scan_width;
    int scan_height;
    stg_meters_t range_max;
  } stg_blobfinder_config_t;
  
  typedef struct
  {
    int channel;
    stg_color_t color;
    int xpos, ypos;   // all values are in pixels
    //int width, height;
    int left, top, right, bottom;
    int area;
    stg_meters_t range;
  } stg_blobfinder_blob_t;


// ENERGY model --------------------------------------------------------------
  
  typedef struct
  {
    stg_joules_t stored;

    stg_bool_t charging;

    stg_meters_t range;

    GPtrArray* connections;
  } stg_energy_data_t;

  typedef struct
  {
    stg_joules_t capacity;

    stg_watts_t give_rate;

    stg_watts_t take_rate;

    stg_meters_t probe_range;
    
    stg_bool_t give;

  } stg_energy_config_t;

  // there is currently no energy command packet

// BLINKENLIGHT -------------------------------------------------------

  //typedef struct
  //{
  //int enable;
  //stg_msec_t period;
  //} stg_blinkenlight_t;

// PTZ MODEL --------------------------------------------------------
  
  typedef stg_ptz_t stg_ptz_cmd_t;

  typedef stg_ptz_t stg_ptz_data_t;

  typedef struct
  {
    stg_ptz_t min; 
    stg_ptz_t max; 
    stg_ptz_t goal; 
    stg_ptz_t speed; 
    stg_bool_t position_mode; 
  } stg_ptz_config_t;


  // LASER MODEL --------------------------------------------------------
  
  typedef struct
  {
    stg_meters_t range; 
    double reflectance; 
  } stg_laser_sample_t;
  
  typedef struct
  {
    stg_radians_t fov; 
    stg_meters_t range_max; 
    stg_meters_t range_min; 

    int samples;

    int resolution;
    } stg_laser_config_t;
    
  // GRIPPER MODEL --------------------------------------------------------
  
  typedef enum {
    STG_GRIPPER_PADDLE_OPEN = 0, // default state
    STG_GRIPPER_PADDLE_CLOSED, 
    STG_GRIPPER_PADDLE_OPENING,
    STG_GRIPPER_PADDLE_CLOSING,
  } stg_gripper_paddle_state_t;

  typedef enum {
    STG_GRIPPER_LIFT_DOWN = 0, // default state
    STG_GRIPPER_LIFT_UP, 
    STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state
    STG_GRIPPER_LIFT_DOWNING, 
  } stg_gripper_lift_state_t;
  
  typedef enum {
    STG_GRIPPER_CMD_NOP = 0, // default state
    STG_GRIPPER_CMD_OPEN, 
    STG_GRIPPER_CMD_CLOSE,
    STG_GRIPPER_CMD_UP, 
    STG_GRIPPER_CMD_DOWN    
  } stg_gripper_cmd_type_t;
  
  typedef struct
  {
    stg_size_t paddle_size; 

    stg_gripper_paddle_state_t paddles; 
    stg_gripper_lift_state_t lift;

    double paddle_position; 
    double lift_position; 

    stg_meters_t inner_break_beam_inset; 
    stg_meters_t outer_break_beam_inset; 
    stg_bool_t paddles_stalled; // true iff some solid object stopped
                                // the paddles closing or opening
    
    GSList *grip_stack;  
    int grip_stack_size; 

    double close_limit; 

  } stg_gripper_config_t;

  typedef struct
  {
    stg_gripper_cmd_type_t cmd;
    int arg;
  } stg_gripper_cmd_t;


  typedef struct
  {
    stg_gripper_paddle_state_t paddles; 
    stg_gripper_lift_state_t lift;
    
    double paddle_position; 
    double lift_position; 

    stg_bool_t inner_break_beam; 
    stg_bool_t outer_break_beam; 
    
    stg_bool_t paddle_contacts[2]; 

    stg_bool_t paddles_stalled; // true iff some solid object stopped
                                // the paddles closing or opening

    int stack_count; 


  } stg_gripper_data_t;


  // FIDUCIAL MODEL --------------------------------------------------------
  
  typedef struct
  {
    stg_meters_t max_range_anon; //< maximum detection range
    stg_meters_t max_range_id; 
    stg_meters_t min_range; 
    stg_radians_t fov; 
    stg_radians_t heading; 
    
    char* key;
  } stg_fiducial_config_t;
  
  typedef struct
  {
    stg_meters_t range; 
    stg_radians_t bearing; 
    stg_pose_t geom; 
    int id; 
    
  } stg_fiducial_t;

  // RANGER MODEL --------------------------------------------------------

  typedef struct
  {
    stg_pose_t pose;
    stg_size_t size;
    stg_bounds_t bounds_range;
    stg_radians_t fov;
    int ray_count;
  } stg_ranger_config_t;
  
  typedef struct
  {
    stg_meters_t range;
    //double error; // TODO
  } stg_ranger_sample_t;
  
  // BUMPER MODEL --------------------------------------------------------

  typedef struct
  {
    stg_pose_t pose;
    stg_meters_t length;
  } stg_bumper_config_t;
  
  typedef struct
  {
    stg_model_t* hit;
    stg_point_t hit_point;
  } stg_bumper_sample_t;
  
  // POSITION MODEL --------------------------------------------------------
  
  typedef enum
    { STG_POSITION_CONTROL_VELOCITY, STG_POSITION_CONTROL_POSITION }
  stg_position_control_mode_t;
  
#define STG_POSITION_CONTROL_DEFAULT STG_POSITION_CONTROL_VELOCITY
  
  typedef enum
    { STG_POSITION_LOCALIZATION_GPS, STG_POSITION_LOCALIZATION_ODOM }
  stg_position_localization_mode_t;
  
#define STG_POSITION_LOCALIZATION_DEFAULT STG_POSITION_LOCALIZATION_GPS
  
  typedef enum
    { STG_POSITION_DRIVE_DIFFERENTIAL, STG_POSITION_DRIVE_OMNI, STG_POSITION_DRIVE_CAR }
  stg_position_drive_mode_t;
  
#define STG_POSITION_DRIVE_DEFAULT STG_POSITION_DRIVE_DIFFERENTIAL
  
  typedef struct
  {
    stg_meters_t x,y,a;
    stg_position_control_mode_t mode;
  } stg_position_cmd_t;
  
  typedef struct
  {
    stg_pose_t pose; 
    stg_pose_t pose_error; 
    stg_pose_t origin; 
    stg_velocity_t velocity; 
    stg_velocity_t integration_error; 
    //stg_bool_t stall; ///< TRUE iff the robot can't move due to a collision
    stg_position_localization_mode_t localization; 
    stg_msec_t watchdog_timeout; 
  } stg_position_data_t;
  
  typedef struct
  {
    stg_position_drive_mode_t drive_mode;
    stg_position_localization_mode_t localization_mode;
  } stg_position_cfg_t;

  void stg_model_position_set_odom( stg_model_t* mod, stg_pose_t* odom ); 

  // WIFI MODEL --------------------------------------------------------

  typedef struct
  {
                char essid[32];
                char mac[32];
    char ip[32];
    double power;
    double sensitivity;
                double freq;
    stg_meters_t range;
                char model[32];
                double plc; // distance power loss coefficient
                double ple; // path loss distance exponent
                double sigma; // standard derivation of gaussian random variable
                double range_db; // setting the individual ranges to render in render_cfg 
                double wall_factor;
  } stg_wifi_config_t;

  typedef struct
  {
    // this is how an entry in neighbours looks like
    stg_pose_t pose; // global pose of corresponding neighbour
                char essid[32];
                char mac[32];    
                char ip[32]; // IP adress of corresponding card
                double freq;
    double db; // signal-strength
                
  } stg_wifi_sample_t;

  typedef struct
  {
    // Simulated wifi data goes here.  E.g., for each neighbor within
    // range, record the corresponding signal strength.
  GArray* neighbours;
  } stg_wifi_data_t;

  // SPEECH MODEL --------------------------------------------------------

#define STG_SPEECH_MAX_STRING_LEN 256

  typedef enum {
    STG_SPEECH_CMD_NOP = 0, // default state
    STG_SPEECH_CMD_SAY
  } stg_speech_cmd_type_t;

  typedef struct
  {
    char string[STG_SPEECH_MAX_STRING_LEN];
  } stg_speech_config_t;

  typedef struct
  {
    char string[STG_SPEECH_MAX_STRING_LEN];
  } stg_speech_data_t;

  typedef struct
  {
    stg_speech_cmd_type_t cmd;
    char string[STG_SPEECH_MAX_STRING_LEN];
  } stg_speech_cmd_t;

  // AUDIO MODEL --------------------------------------------------------

#define STG_AUDIO_MAX_STRING_LEN 256

  typedef enum {
    STG_AUDIO_CMD_NOP = 0, // default state
    STG_AUDIO_CMD_SAY
  } stg_audio_cmd_type_t;

  typedef struct
  {
    char string[STG_AUDIO_MAX_STRING_LEN];
    char say_string[STG_AUDIO_MAX_STRING_LEN];
    stg_msec_t say_period;
    stg_msec_t say_last_time;
  } stg_audio_config_t;

  typedef struct
  {
    char string[STG_AUDIO_MAX_STRING_LEN];
    char recv[STG_AUDIO_MAX_STRING_LEN];
    GList *audio_paths;
  } stg_audio_data_t;

  typedef struct
  {
    stg_audio_cmd_type_t cmd;
    char string[STG_AUDIO_MAX_STRING_LEN];
  } stg_audio_cmd_t;

  // end the group of all models
// MACROS ------------------------------------------------------
// Some useful macros
  
  stg_color_t stg_lookup_color(const char *name);

  void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 );

  // PRETTY PRINTING -------------------------------------------------
  
  void stg_print_err( const char* err );
  void stg_print_geom( stg_geom_t* geom );
  void stg_print_pose( stg_pose_t* pose );
  void stg_print_velocity( stg_velocity_t* vel );
  void stg_print_gripper_config( stg_gripper_config_t* slc );
  void stg_print_laser_config( stg_laser_config_t* slc );
  


#define PRECISION 100000.0

#define EQ(A,B) ((lrint(A*PRECISION))==(lrint(B*PRECISION)))

#define LT(A,B) ((lrint(A*PRECISION))<(lrint(B*PRECISION)))

#define GT(A,B) ((lrint(A*PRECISION))>(lrint(B*PRECISION)))

#define GTE(A,B) ((lrint(A*PRECISION))>=(lrint(B*PRECISION)))

#define LTE(A,B) ((lrint(A*PRECISION))<=(lrint(B*PRECISION)))



#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

#define MILLION 1e6
#define BILLION 1e9

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifndef TWOPI
#define TWOPI (2.0*M_PI)
#endif
  
#ifndef RTOD

#define RTOD(r) ((r) * 180.0 / M_PI)
#endif
  
#ifndef DTOR

#define DTOR(d) ((d) * M_PI / 180.0)
#endif
  
#ifndef NORMALIZE

#define NORMALIZE(z) atan2(sin(z), cos(z))
#endif

// end doc group libstage_utilities
#ifdef __cplusplus
}
#endif 

// end documentation group libstage
#endif

---