Stage

• Frontpage
• Contents
• Release

User

• Introduction
• Installation
• Help
• FAQ

  ---

• World
• Window
• Model
  • blobfinder
  • fiducial
  • gripper
  • laser
  • position
  • ptz
  • 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.179 2006/02/28 05:11:57 rtv Exp $
 */


#include "config.h" // results of autoconf's system configuration tests
#include "replace.h" // Stage's implementations of missing system calls

#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 

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

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


  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_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_ptz_config_t;


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

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

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


  // Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
#define PRINT_ERR2(m,a,b) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
#define PRINT_ERR3(m,a,b,c) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_ERR4(m,a,b,c,d) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_ERR5(m,a,b,c,d,e) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

  // Warning macros
#define PRINT_WARN(m) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_WARN1(m,a) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
#define PRINT_WARN2(m,a,b) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
#define PRINT_WARN3(m,a,b,c) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_WARN4(m,a,b,c,d) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_WARN5(m,a,b,c,d,e) printf( "\033[44mwarn\033[0m: "m" (%s %s)\n", a, b, c, d, e, __FILE__, __FUNCTION__)

  // Message macros
#ifdef DEBUG
#define PRINT_MSG(m) printf( "Stage: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_MSG1(m,a) printf( "Stage: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
#define PRINT_MSG2(m,a,b) printf( "Stage: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
#define PRINT_MSG3(m,a,b,c) printf( "Stage: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m" (%s %s)\n", a, b, c, d, __FILE__, __FUNCTION__)
#define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m" (%s %s)\n", a, b, c, d, e,__FILE__, __FUNCTION__)
#else
#define PRINT_MSG(m) printf( "Stage: "m"\n" )
#define PRINT_MSG1(m,a) printf( "Stage: "m"\n", a)
#define PRINT_MSG2(m,a,b) printf( "Stage: "m"\n,", a, b )
#define PRINT_MSG3(m,a,b,c) printf( "Stage: "m"\n", a, b, c )
#define PRINT_MSG4(m,a,b,c,d) printf( "Stage: "m"\n", a, b, c, d )
#define PRINT_MSG5(m,a,b,c,d,e) printf( "Stage: "m"\n", a, b, c, d, e )
#endif

  // DEBUG macros
#ifdef DEBUG
#define PRINT_DEBUG(m) printf( "debug: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_DEBUG1(m,a) printf( "debug: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)    
#define PRINT_DEBUG2(m,a,b) printf( "debug: "m" (%s %s)\n", a, b, __FILE__, __FUNCTION__) 
#define PRINT_DEBUG3(m,a,b,c) printf( "debug: "m" (%s %s)\n", a, b, c, __FILE__, __FUNCTION__)
#define PRINT_DEBUG4(m,a,b,c,d) printf( "debug: "m" (%s %s)\n", a, b, c ,d, __FILE__, __FUNCTION__)
#define PRINT_DEBUG5(m,a,b,c,d,e) printf( "debug: "m" (%s %s)\n", a, b, c ,d, e, __FILE__, __FUNCTION__)
#else
#define PRINT_DEBUG(m)
#define PRINT_DEBUG1(m,a)
#define PRINT_DEBUG2(m,a,b)
#define PRINT_DEBUG3(m,a,b,c)
#define PRINT_DEBUG4(m,a,b,c,d)
#define PRINT_DEBUG5(m,a,b,c,d,e)
#endif


#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

---