Gazebo

gazebo.h

00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003  
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 
00022 /* Desc: External interfaces for Gazebo
00023  * Author: Andrew Howard
00024  * Date: 6 Apr 2003
00025  * SVN: $Id: gazebo.h 6338 2008-04-16 17:24:32Z natepak $
00026  */
00027 
00028 #ifndef GAZEBO_H
00029 #define GAZEBO_H
00030 
00031 #include <string>
00032 #include <sys/types.h>
00033 #include <stdlib.h>
00034 #include <stdint.h>
00035 
00036 #include "IfaceFactory.hh"
00037 
00038 namespace gazebo
00039 {
00040 
00041 
00042 /***************************************************************************
00043  * Constants, etc
00044  **************************************************************************/
00045 
00048 
00050 #define LIBGAZEBO_VERSION 0x070
00051 
00053 
00056 
00058 class Vec3
00059 {
00061   public: double x;
00062 
00064   public: double y;
00065 
00067   public: double z;
00068 };
00069 
00070 
00072 class Pose
00073 {
00075   public: Vec3 pos;
00076 
00078   public: double roll;
00079 
00081   public: double pitch;
00082 
00084   public: double yaw; 
00085 };
00086 
00088 class Color
00089 {
00091   public: float r;
00092 
00094   public: float g;
00095 
00097   public: float b;
00098 
00100   public: float a;
00101 };
00102 
00103 /***************************************************************************/
00104 
00105 
00106 /***************************************************************************/
00118 
00119 class Server
00120 {
00122   public: Server();
00123 
00125   public: virtual ~Server();
00126 
00128   public: void Init(int serverId, int force);
00129 
00131   public: void Fini();
00132 
00134   public: void Post();
00135 
00136   private: void SemInit(int force);
00137   private: void SemFini();
00138   private: void SemPost();
00139 
00141   public: int serverId;
00142 
00144   public: std::string filename;
00145 
00147   public: int semKey;
00148 
00150   public: int semId;
00151 };
00152 
00154 /***************************************************************************/
00155 
00156   
00157 /***************************************************************************/
00167 
00168 #define GZ_SEM_KEY 0x135135FA
00169   
00175 #define GZ_CLIENT_ID_USER_FIRST 0x00
00176 #define GZ_CLIENT_ID_USER_LAST  0x07
00177 #define GZ_CLIENT_ID_WXGAZEBO   0x08
00178 #define GZ_CLIENT_ID_PLAYER     0x09
00179 
00180   
00182 class Client 
00183 {
00185   public: Client();
00186 
00188   public: virtual ~Client();
00189 
00192   public: void Query(int server_id);
00193 
00196   public: void Connect(int server_id);
00197 
00201   public: void ConnectWait(int server_id, int client_id);
00202 
00204   public: void Disconnect();
00205 
00207   public: void Wait();
00208 
00209   private: void SemQuery(int server_id);
00210   private: void SemInit();
00211   private: void SemFini();
00212   private: void SemWait();
00213 
00215   public: int serverId;
00216 
00218   public: int clientId;
00219 
00221   public: std::string filename;
00222 
00224   public: int semKey;
00225 
00227   public: int semId;
00228 };
00229 
00232 /***************************************************************************/
00242 
00243 #define GAZEBO_MAX_MODEL_TYPE 128
00244 
00246 class Iface
00247 {
00251   public: Iface(const std::string &type, size_t size);
00252 
00254   public: virtual ~Iface();
00255 
00259   public: virtual void Create(Server *server, std::string id);
00260 
00267   public: void Create(Server *server, std::string id,
00268               const std::string &modelType, int modelId, 
00269               int parentModelId);
00270 
00272   public: void Destroy();
00273 
00277   public: virtual void Open(Client *client, std::string id);
00278 
00280   public: virtual void Close();
00281 
00285   public: int Lock(int blocking);
00286 
00288   public: int Unlock();
00289 
00291   public: void Post();
00292 
00294   public: int GetOpenCount();
00295 
00298   public: std::string GetType() const;
00299 
00300   private: std::string Filename(std::string id);
00301 
00303   public: Server *server;
00304 
00306   public: Client *client;
00307 
00309   public: int mmapFd;
00310 
00312   public: void *mMap;
00313 
00315   public: std::string filename;
00316 
00318   protected: std::string type;
00319 
00320   private: bool creator;
00321 
00322   private: size_t size;
00323 };
00324 
00325 class GazeboData
00326 {
00328   public: int openCount;
00329 
00330   public: double time;
00331 
00332   public: int version;
00333 
00334   public: size_t size;
00335 
00337   public: int modelId;
00338 
00340   public: int parentModelId;
00341 
00343   public: std::string modelType;
00344 
00345 
00346 };
00347 
00351 /***************************************************************************/
00354 
00365 
00366 class SimulationData
00367 {
00368   public: GazeboData head;
00369 
00371   public: double simTime;
00372 
00374   public: double pauseTime;
00375 
00377   public: double realTime;
00378 
00380   public: int state;
00381   
00384   public: bool pause;
00385 
00387   public: int reset;
00388 
00390   public: int save;
00391 
00393   public: char model_name[512];
00394 
00399   public: char model_req[32];
00400 
00403   public: Pose model_pose;
00404 };
00405 
00407 class SimulationIface : public Iface
00408 {
00410   public: SimulationIface(): Iface("simulation",sizeof(SimulationIface)+sizeof(SimulationData)) {}
00411 
00413   public: virtual ~SimulationIface() {this->data = NULL;}
00414 
00418   public: virtual void Create(Server *server, std::string id)
00419           {
00420             Iface::Create(server,id); 
00421             this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); 
00422           }
00423 
00427   public: virtual void Open(Client *client, std::string id)
00428           {
00429             Iface::Open(client,id); 
00430             this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); 
00431           }
00432 
00434   public: SimulationData *data;
00435 };
00436 
00438 
00439 
00440 
00441 /***************************************************************************/
00444 
00456 #define GAZEBO_MAX_URL_SIZE 256
00457 
00459 class AudioData
00460 {
00461   public: GazeboData head;
00462 
00464   public: double time;
00465 
00467   public: bool loop;
00468 
00470   public: float gain;
00471 
00473   public: bool stream;
00474 
00476   public: int cmd_play;
00477 
00479   public: int cmd_pause;
00480 
00482   public: int cmd_stop;
00483   
00485   public: int cmd_reset;
00486   
00488   public: int state;
00489  
00491   public: char url[GAZEBO_MAX_URL_SIZE];
00492 
00494   public: Pose audio_pose;
00495   
00496 };
00497 
00499 class AudioIface : public Iface
00500 {
00502   public: AudioIface():Iface("audio", sizeof(AudioIface)+sizeof(AudioData)) {}
00503 
00505   public: virtual ~AudioIface() {this->data = NULL;}
00506 
00510   public: virtual void Create(Server *server, std::string id)
00511           {
00512             Iface::Create(server,id); 
00513             this->data = (AudioData*)this->mMap; 
00514           }
00515 
00519   public: virtual void Open(Client *client, std::string id)
00520           {
00521             Iface::Open(client,id); 
00522             this->data = (AudioData*)this->mMap; 
00523           }
00524 
00526   public: AudioData *data;
00527 };
00528 
00529 
00531 
00532 
00533 
00534 
00535 /***************************************************************************/
00538 
00552 
00553 #define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
00554 
00556 class CameraData
00557 {
00558   public: GazeboData head;
00559 
00561   public: unsigned int width;
00562 
00564   public: unsigned int height;
00565 
00567   public: unsigned int image_size;
00568 
00570   public: unsigned char image[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
00571 
00573   public: double hfov;
00574 
00576   public: Pose camera_pose;
00577   
00578 };
00579 
00581 class CameraIface : public Iface
00582 {
00584   public: CameraIface():Iface("camera", sizeof(CameraIface)+sizeof(CameraData)) {}
00585 
00587   public: virtual ~CameraIface() {this->data = NULL;}
00588 
00592   public: virtual void Create(Server *server, std::string id)
00593           {
00594             Iface::Create(server,id); 
00595             this->data = (CameraData*)this->mMap; 
00596           }
00597 
00601   public: virtual void Open(Client *client, std::string id)
00602           {
00603             Iface::Open(client,id); 
00604             this->data = (CameraData*)this->mMap; 
00605           }
00606 
00608   public: CameraData *data;
00609 };
00610 
00611 
00613 
00614 
00615 
00616 /***************************************************************************/
00619 
00631 
00632 class PositionData 
00633 {
00634   public: GazeboData head;
00635 
00637   public: Pose pose;
00638 
00640   public: Pose velocity;
00641 
00643   public: int stall;
00644 
00646   public: int cmdEnableMotors;
00647   
00649   public: Pose cmdVelocity;
00650 };
00651 
00653 class PositionIface : public Iface
00654 {
00656   public: PositionIface():Iface("position", sizeof(PositionIface)+sizeof(PositionData)) {}
00657 
00659   public: virtual ~PositionIface() {this->data = NULL;}
00660 
00664   public: virtual void Create(Server *server, std::string id)
00665           {
00666             Iface::Create(server,id); 
00667             this->data = (PositionData*)this->mMap; 
00668           }
00669 
00673   public: virtual void Open(Client *client, std::string id)
00674           {
00675             Iface::Open(client,id); 
00676             this->data = (PositionData*)this->mMap; 
00677           }
00678 
00680   public: PositionData *data;
00681 };
00682 
00684 
00685 
00686 /***************************************************************************/
00689 
00700 
00701 #define GAZEBO_GRAPHICS3D_MAX_POINTS 1024
00702 
00704 class Graphics3dData
00705 {
00707   enum DrawMode {POINTS, LINES, LINE_STRIP, LINE_LOOP, TRIANGLES, TRIANGLE_STRIP, TRIANGLE_FAN, QUADS, QUAD_STRIP, POLYGON};
00708 
00710   public: DrawMode drawmode;
00711 
00713   public: unsigned int point_count; 
00714 
00716   public: Vec3 points[GAZEBO_GRAPHICS3D_MAX_POINTS];
00717 
00719   public: Color color;
00720 };
00721 
00723 class Graphics3dIface : public Iface
00724 {
00725 
00727   public: Graphics3dIface():Iface("graphics3d", sizeof(Graphics3dIface)+sizeof(Graphics3dData)) {}
00728 
00730   public: virtual ~Graphics3dIface() {this->data = NULL;}
00731 
00735   public: virtual void Create(Server *server, std::string id)
00736           {
00737             Iface::Create(server,id); 
00738             this->data = (Graphics3dData*)this->mMap; 
00739           }
00740 
00744   public: virtual void Open(Client *client, std::string id)
00745           {
00746             Iface::Open(client,id); 
00747             this->data = (Graphics3dData*)this->mMap; 
00748           }
00749 
00750 
00752   public: Graphics3dData *data;
00753 };
00754 
00755 
00757 
00758 
00759 
00760   
00761 /***************************************************************************/
00764 
00776 
00777 #define GZ_LASER_MAX_RANGES 1024
00778 
00780 class LaserData
00781 {
00782   public: GazeboData head;
00783   
00785   public: double min_angle;
00786 
00788   public: double max_angle;
00789 
00791   public: double res_angle;
00792 
00794   public: double max_range;
00795 
00797   public: int range_count;
00798   
00800   public: double ranges[GZ_LASER_MAX_RANGES];
00801 
00803   public: int intensity[GZ_LASER_MAX_RANGES];
00804   
00806   public: int cmd_new_angle;
00807 
00809   public: int cmd_new_length;
00810 
00812   public: double cmd_max_range;
00813 
00815   public: double cmd_min_angle;
00816 
00818   public: double cmd_max_angle;
00819 
00821   public: int cmd_range_count;
00822 };
00823 
00825 class LaserIface : public Iface
00826 {
00828   public: LaserIface():Iface("laser", sizeof(LaserIface)+sizeof(LaserData)) {}
00829 
00831   public: virtual ~LaserIface() {this->data = NULL;}
00832 
00836   public: virtual void Create(Server *server, std::string id)
00837           {
00838             Iface::Create(server,id); 
00839             this->data = (LaserData*)this->mMap; 
00840           }
00841 
00845   public: virtual void Open(Client *client, std::string id)
00846           {
00847             Iface::Open(client,id); 
00848             this->data = (LaserData*)this->mMap; 
00849           }
00850 
00852   public: LaserData *data;
00853 };
00854 
00856 
00857 
00858 
00859 /***************************************************************************/
00862 
00874 
00875 #define GZ_FIDUCIAL_MAX_FIDS 401
00876 
00878 class FiducialFid
00879 {
00880 
00882   public: int id;
00883 
00885   public: Pose pose;
00886 };
00887 
00889 class FiducialData
00890 {
00891   public: GazeboData head;
00892 
00894   public: int count;
00895 
00897   public: FiducialFid fids[GZ_FIDUCIAL_MAX_FIDS];
00898 };
00899 
00901 class FiducialIface : public Iface
00902 {
00904   public: FiducialIface():Iface("fiducial", sizeof(FiducialIface)+sizeof(FiducialData)) {}
00905 
00907   public: virtual ~FiducialIface() {this->data = NULL;}
00908 
00910   public: virtual void Create(Server *server, std::string id)
00911           {
00912             Iface::Create(server,id); 
00913             this->data = (FiducialData*)this->mMap; 
00914           }
00915 
00917   public: virtual void Open(Client *client, std::string id)
00918           {
00919             Iface::Open(client,id); 
00920             this->data = (FiducialData*)this->mMap; 
00921           }
00922 
00924   public: FiducialData *data;
00925 };
00926 
00928 
00929 
00930 /***************************************************************************/
00933 
00943 
00944 class FactoryData 
00945 {
00946   public: GazeboData head;
00947 
00949   public: uint8_t newModel[4096];
00950 
00952   public: uint8_t deleteModel[512];
00953 };
00954 
00956 class FactoryIface : public Iface
00957 {
00959   public: FactoryIface():Iface("factory", sizeof(FactoryIface)+sizeof(FactoryData)) {}
00960 
00962   public: virtual ~FactoryIface() {this->data = NULL;}
00963 
00965   public: virtual void Create(Server *server, std::string id)
00966           {
00967             Iface::Create(server,id); 
00968             this->data = (FactoryData*)this->mMap; 
00969           }
00970 
00972   public: virtual void Open(Client *client, std::string id)
00973           {
00974             Iface::Open(client,id); 
00975             this->data = (FactoryData*)this->mMap; 
00976           }
00977 
00979   public: FactoryData *data;
00980 };
00981 
00983 
00984 
00985 /***************************************************************************/
00988 
01000 #define GAZEBO_GRIPPER_STATE_OPEN 1
01001 
01002 #define GAZEBO_GRIPPER_STATE_CLOSED 2
01003 
01004 #define GAZEBO_GRIPPER_STATE_MOVING 3
01005 
01006 #define GAZEBO_GRIPPER_STATE_ERROR 4
01007 
01009 #define GAZEBO_GRIPPER_CMD_OPEN 1
01010 
01011 #define GAZEBO_GRIPPER_CMD_CLOSE 2
01012 
01013 #define GAZEBO_GRIPPER_CMD_STOP 3
01014 
01015 #define GAZEBO_GRIPPER_CMD_STORE 4
01016 
01017 #define GAZEBO_GRIPPER_CMD_RETRIEVE 5
01018 
01019 
01021 class GripperData
01022 {
01023   public: GazeboData head;
01024 
01026   public: int cmd;
01027 
01029   public: int state;
01030 
01032   public: int grip_limit_reach;
01033 
01035   public: int lift_limit_reach;
01036 
01038   public: int outer_beam_obstruct;
01039 
01041   public: int inner_beam_obstruct;
01042 
01044   public: int left_paddle_open;
01045 
01047   public: int right_paddle_open;
01048 
01050   public: int lift_up;
01051 
01053   public: int lift_down;
01054 };
01055 
01057 class GripperIface : public Iface
01058 {
01060   public: GripperIface():Iface("gripper", sizeof(GripperIface)+sizeof(GripperData)) {}
01061 
01063   public: virtual ~GripperIface() {this->data = NULL;}
01064 
01066   public: virtual void Create(Server *server, std::string id)
01067           {
01068             Iface::Create(server,id); 
01069             this->data = (GripperData*)this->mMap; 
01070           }
01071 
01073   public: virtual void Open(Client *client, std::string id)
01074           {
01075             Iface::Open(client,id); 
01076             this->data = (GripperData*)this->mMap; 
01077           }
01078 
01080   public: GripperData *data;
01081 };
01082 
01084 
01085 
01086 
01087 /***************************************************************************/
01090 
01099 
01100 #define GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS 16
01101 #define GAZEBO_ACTARRAY_JOINT_POSITION_MODE 0
01102 #define GAZEBO_ACTARRAY_JOINT_SPEED_MODE 1
01103 #define GAZEBO_ACTARRAY_JOINT_CURRENT_MODE 2
01104 
01105 //Actuator states
01107 #define GAZEBO_ACTARRAY_ACTSTATE_IDLE     1
01108 
01110 #define GAZEBO_ACTARRAY_ACTSTATE_MOVING   2
01111 
01113 #define GAZEBO_ACTARRAY_ACTSTATE_BRAKED   3
01114 
01116 #define GAZEBO_ACTARRAY_ACTSTATE_STALLED  4
01117 
01119 #define GAZEBO_ACTARRAY_TYPE_LINEAR       1
01121 #define GAZEBO_ACTARRAY_TYPE_ROTARY       2
01122 
01124 #define GAZEBO_ACTARRAY_POWER_REQ         1
01126 #define GAZEBO_ACTARRAY_BRAKES_REQ        2
01128 #define GAZEBO_ACTARRAY_GET_GEOM_REQ      3
01130 #define GAZEBO_ACTARRAY_SPEED_REQ         4
01131 
01133 #define GAZEBO_ACTARRAY_POS_CMD           1
01135 #define GAZEBO_ACTARRAY_SPEED_CMD         2
01137 #define GAZEBO_ACTARRAY_HOME_CMD          3
01138 
01139 
01141 class ActarrayActuatorGeom
01142 {
01143 
01145 #define GAZEBO_ACTARRAY_DATA_STATE        1
01146 
01147 
01149   public: uint8_t type;
01150 
01152   public: float min;
01153 
01155   public: float center;
01156 
01158   public: float max;
01159 
01161   public: float home;
01162 
01164   public: float config_speed;
01165 
01167   public: float max_speed;
01168 
01170   public: uint8_t hasbrakes;
01171 };
01172 
01174 class ActarrayActuator
01175 {
01177   public: float position;
01179   public: float speed;
01181   public: uint8_t state;
01182 
01183 }; 
01184 
01186 class ActarrayData
01187 {
01188   public: GazeboData head;
01189 
01191   public: unsigned int actuators_count;
01192   
01194   public: ActarrayActuator actuators[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS];
01195   
01197   public: ActarrayActuatorGeom actuator_geoms[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS];
01198   
01200   public: float cmd_pos[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS];
01201   
01203   public: float cmd_speed[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS];
01204   
01206   public: int bad_cmd;
01207 
01209   public: bool new_cmd;
01210   
01212   public: unsigned int joint_mode[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS];
01213   
01214 };
01215 
01217 class ActarrayIface : public Iface
01218 {
01220   public: ActarrayIface():Iface("actarray", sizeof(ActarrayIface)+sizeof(ActarrayData)) {}
01221 
01223   public: virtual ~ActarrayIface() {this->data = NULL;}
01224 
01228   public: virtual void Create(Server *server, std::string id)
01229           {
01230            Iface::Create(server,id); 
01231            this->data = (ActarrayData*)this->mMap; 
01232           }
01233 
01237   public: virtual void Open(Client *client, std::string id)
01238           {
01239             Iface::Open(client,id); 
01240             this->data = (ActarrayData*)this->mMap; 
01241           }
01242 
01244   public: ActarrayData *data;
01245 };
01246 
01248 
01249 
01250 
01251 /***************************************************************************/
01254 
01263 
01264 class PTZData
01265 {
01266   public: GazeboData head;
01267 
01269   public: double pan;
01270 
01272   public: double tilt;
01273 
01275   public: double zoom;
01276 
01278   public: double cmd_pan;
01279 
01281   public: double cmd_tilt;
01282 
01284   public: double cmd_zoom;
01285   
01286 }; 
01287 
01288 
01290 class PTZIface : public Iface
01291 {
01293   public: PTZIface():Iface("ptz", sizeof(PTZIface)+sizeof(PTZData)) {}
01294 
01296   public: virtual ~PTZIface() {this->data = NULL;}
01297 
01299   public: virtual void Create(Server *server, std::string id)
01300           {
01301             Iface::Create(server,id); 
01302             this->data = (PTZData*)this->mMap; 
01303           }
01304 
01306   public: virtual void Open(Client *client, std::string id)
01307           {
01308             Iface::Open(client,id); 
01309             this->data = (PTZData*)this->mMap; 
01310           }
01311 
01313   public: PTZData *data;
01314 };
01315 
01317 
01318 
01319 
01320 /***************************************************************************/
01323 
01331 #define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3
01332 #define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480
01333 
01335 class StereoCameraData
01336 {
01337   public: GazeboData head;
01338 
01340   public: unsigned int width;
01341 
01343   public: unsigned int height;
01344 
01346   public: float farClip;
01347 
01349   public: float nearClip;
01350 
01352   public: unsigned int left_rgb_size;
01353 
01355   public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE];
01356 
01358   public: unsigned int right_rgb_size;
01359 
01361   public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE];
01362 
01364   public: unsigned int left_disparity_size;
01365 
01367   public: float left_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
01368 
01370   public: unsigned int right_disparity_size;
01371 
01373   public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
01374 }; 
01375 
01376 
01378 class StereoCameraIface : public Iface
01379 {
01381   public: StereoCameraIface():Iface("stereo", sizeof(StereoCameraIface)+sizeof(StereoCameraData)) {}
01382 
01384   public: virtual ~StereoCameraIface() {this->data = NULL;}
01385 
01387   public: virtual void Create(Server *server, std::string id)
01388           {
01389             Iface::Create(server,id); 
01390             this->data = (StereoCameraData*)this->mMap; 
01391           }
01392 
01394   public: virtual void Open(Client *client, std::string id)
01395           {
01396             Iface::Open(client,id); 
01397             this->data = (StereoCameraData*)this->mMap; 
01398           }
01399 
01401   public: StereoCameraData *data;
01402 };
01403 
01405 
01406 
01407 }
01408 
01409 
01410 #endif

Last updated Aug 04 2007