Stage

• Frontpage
• Contents
• Release

User

• Stage
• Installation
• FAQ
• Help

  ---

• World
• Window
• Model
  • blobfinder
  • fiducial
  • laser
  • position
  • ranger

  ---

• 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.158 2005/08/11 19:56:54 rtv Exp $
 */


#include <stdlib.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netdb.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h> // for portable int types eg. uint32_t
#include <sys/types.h>
#include <sys/time.h>
#include <assert.h>
#include <pthread.h>
#include <semaphore.h>

#include <glib.h> // we use GLib's data structures extensively
#include <rtk.h> // and graphics stuff pulled from Andrew Howard's RTK2 library

#ifdef __cplusplus
extern "C" {
#endif 

#include "config.h"
#include "replace.h"

  // TODO - fix this up
#define FiducialNone 0
  

  // Basic self-describing measurement types. All packets with real
  // measurements are specified in these terms so changing types here
  // should work throughout the code If you change these, be sure to
  // change the byte-ordering macros below accordingly.

  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;
  

  // PRETTY PRINTING -------------------------------------------------
  
  void stg_print_geom( stg_geom_t* geom );

  void stg_err( const char* err );

  // ENERGY --------------------------------------------------------------

  typedef struct
  {
    stg_joules_t stored;

    stg_joules_t capacity;

    stg_joules_t input_joules;

    stg_joules_t output_joules;

    stg_watts_t input_watts;

    stg_watts_t output_watts;

    stg_bool_t charging;

    stg_meters_t range;
  } stg_energy_data_t;

  typedef struct
  {
    stg_joules_t capacity;

    stg_watts_t give;

    stg_watts_t take;

    stg_meters_t probe_range;
  } stg_energy_config_t;

  // there is currently no energy command packet

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

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

  // GRIPPER ------------------------------------------------------------

  // Possible Gripper return values
  //typedef enum 
  //{
  //  GripperDisabled = 0,
  //  GripperEnabled
  //} stg_gripper_return_t;

  // GUIFEATURES -------------------------------------------------------
  
  // 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 struct */
/*   { */
/*     uint8_t show_data; */
/*     uint8_t show_cfg; */
/*     uint8_t show_cmd; */
    
/*     uint8_t nose; */
/*     uint8_t grid; */
/*     //uint8_t boundary; */
/*     uint8_t outline; */
/*     stg_movemask_t movemask; */
/*   } stg_guifeatures_t; */


  // LASER ------------------------------------------------------------

  typedef enum 
    {
      LaserTransparent, 
      LaserVisible, 
      LaserBright  
    } stg_laser_return_t;


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

  void stg_quit_request( void );


  // UTILITY STUFF ----------------------------------------------------

  const char* stg_version_string( void );

  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 );

  // 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 );

  // POLYGONS ---------------------------------------------------------

  typedef struct
  {
    GArray* points;
    
    stg_bool_t filled; 
    
    stg_color_t color;
  } 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 util documentation group
  // end property typedefs -------------------------------------------------


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

  typedef struct _stg_world stg_world_t;


  typedef struct _stg_model stg_model_t; // defined in stage_internal.h

  //  MODEL --------------------------------------------------------
    
  // group the docs of all the model types
#define STG_PROPNAME_MAX 128




  
  typedef int (*stg_property_callback_t)(stg_model_t* mod, char* propname, void* data, size_t len, void* userdata );
  
  
  typedef int(*stg_model_initializer_t)(stg_model_t*);
  

  typedef struct
  {
    stg_property_callback_t callback;
    void* arg;
  } stg_cbarg_t;

  stg_model_t* stg_model_create(  stg_world_t* world,
                                  stg_model_t* parent, 
                                  stg_id_t id, 
                                  char* token,
                                  stg_model_initializer_t initializer );

  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

  int stg_model_set_global_pose( stg_model_t* mod, stg_pose_t* gpose );
  
  int stg_model_set_velocity( stg_model_t* mod, stg_velocity_t* vel );
 
  // TODO?
  //void stg_model_lock( stg_model_t* mod );
  
  //void stg_model_unlock( stg_model_t* mod );

  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_velocity( stg_model_t* mod, stg_velocity_t* dest );
  
  void stg_model_set_property( stg_model_t* mod, 
                               const char* prop, 
                               void* data, 
                               size_t len );  
  
  void* stg_model_get_property( stg_model_t* mod, 
                                const char* prop,
                                size_t* len );
  
  void* stg_model_get_property_fixed( stg_model_t* mod, 
                                      const char* name,
                                      size_t size );

  void stg_model_property_refresh( stg_model_t* mod, const char* propname );


  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 );

  // get a copy of the property data - caller must free it
  //int stg_model_copy_property_data( stg_model_t* mod, const char* prop,
  //                       void** data );
  
  int stg_model_add_property_callback( stg_model_t* mod, const char* prop, 
                                       stg_property_callback_t, void* user );
  
  int stg_model_remove_property_callback( stg_model_t* mod, const char* prop, 
                                          stg_property_callback_t );
  
  int stg_model_remove_property_callbacks( stg_model_t* mod, const char* prop );

  void stg_model_print( stg_model_t* mod );
  
  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 );


  void stg_model_add_property_toggles( stg_model_t* mod, 
                                       const char* propname, 
                                       stg_property_callback_t callback_on,
                                       void* arg_on,
                                       stg_property_callback_t callback_off,
                                       void* arg_off,
                                       const char* label,
                                       int enabled );

  int stg_model_fig_clear_cb( stg_model_t* mod, void* data, size_t len, 
                              void* userp );
   // end model doc group

 //  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 );
  

  
  void stg_world_add_property_callback( stg_world_t* world, 
                                        char* propname, 
                                        stg_property_callback_t callback, void*
                                        userdata );
  
  void stg_world_remove_property_callback( stg_world_t* world,
                                           char* propname,
                                           stg_property_callback_t callback );
  
  // 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_radians_t pan, tilt, zoom;
  } 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;

  stg_model_t* stg_blobfinder_create( stg_world_t* world,       
                                      stg_model_t* parent, 
                                      stg_id_t id,  
                                      char* token );   
  // LASER MODEL --------------------------------------------------------
  
  typedef struct
  {
    uint32_t range; 
    char reflectance; 
  } stg_laser_sample_t;
  
  typedef struct
  {
    stg_radians_t fov; 
    stg_meters_t range_max; 
    stg_meters_t range_min; 

    int samples; 
  } stg_laser_config_t;
  
  void stg_print_laser_config( stg_laser_config_t* slc );
  
  stg_model_t* stg_laser_create( stg_world_t* world, 
                                 stg_model_t* parent, 
                                 stg_id_t id,
                                 char* token );

  size_t stg_model_get_data_laser( stg_model_t* mod,
                                   stg_laser_sample_t* data, 
                                   size_t max_samples );
  // 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; 

  } 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 left_paddle_contact[3]; 
    stg_bool_t right_paddle_contact[3]; 
    
    stg_bool_t paddles_stalled; // true iff some solid object stopped
                                // the paddles closing or opening

    int stack_count; 

  } stg_gripper_data_t;


  void stg_print_gripper_config( stg_gripper_config_t* slc );
  
  stg_model_t* stg_gripper_create( stg_world_t* world, 
                                 stg_model_t* parent, 
                                 stg_id_t id,
                                 char* token );
  // FIDUCIAL MODEL --------------------------------------------------------
  
  typedef struct
  {
    stg_meters_t max_range_anon;
    stg_meters_t max_range_id;
    stg_meters_t min_range;
    stg_radians_t fov; // field of view 
    stg_radians_t heading; // center of field of view

  } stg_fiducial_config_t;
  
  typedef struct
  {
    stg_meters_t range; // range to the target
    stg_radians_t bearing; // bearing to the target 
    stg_pose_t geom; // size and relative angle of the target
    int id; // the identifier of the target, or -1 if none can be detected.
    
  } stg_fiducial_t;

  stg_model_t* stg_fiducial_create( stg_world_t* world,  
                                    stg_model_t* parent,  
                                    stg_id_t id, 
                                    char* token );  
  // RANGER MODEL --------------------------------------------------------
  
  typedef struct
  {
    stg_pose_t pose;
    stg_size_t size;
    stg_bounds_t bounds_range;
    stg_radians_t fov;
  } stg_ranger_config_t;
  
  typedef struct
  {
    stg_meters_t range;
    //double error; // TODO
  } stg_ranger_sample_t;
  
  stg_model_t* stg_ranger_create( stg_world_t* world,  
                                  stg_model_t* parent, 
                                  stg_id_t id, 
                                  char* token );
  // POSITION MODEL --------------------------------------------------------
  
  //#define STG_MM_POSITION_RESETODOM 77
  
  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_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; 
    stg_position_localization_mode_t localization; 
  } stg_position_data_t;
  
  typedef int stg_position_stall_t;

  stg_model_t* stg_position_create( stg_world_t* world,  stg_model_t* parent,  stg_id_t id, char* token );
  
  void stg_model_position_set_odom( stg_model_t* mod, stg_pose_t* odom ); 

  // end the group of all models
// MACROS ------------------------------------------------------
// Some useful macros



#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


#ifdef __cplusplus
}
#endif 

// end documentation group libstage
#endif

---