00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef PEOPLEBOT_HH
00026 #define PEOPLEBOT_HH
00027
00028 #include "Body.hh"
00029 #include "Model.hh"
00030
00031
00032 class Body;
00033 class HingeJoint;
00034 class BallJoint;
00035 class RayProximity;
00036
00037 class PeopleBot : public Model
00038 {
00039
00040 public: PeopleBot( World *world );
00041 public: virtual ~PeopleBot();
00042
00043
00044 public: virtual int Load( WorldFile *file, WorldFileNode *node );
00045
00046
00047 public: virtual int Init( WorldFile *file, WorldFileNode *node );
00048
00049
00050 public: virtual int Fini();
00051
00052
00053 public: virtual void Update( double step );
00054
00055
00056 private: int LoadODE( WorldFile *file, WorldFileNode *node );
00057
00058
00059 private: int LoadSonar( WorldFile *file, WorldFileNode *node );
00060
00061
00062 private: void UpdateOdometry( double step );
00063
00064
00065 private: void GetPositionCmd();
00066
00067
00068 private: void PutPositionData();
00069
00070
00071 private: void PutPowerData();
00072
00073
00074 private: void PutSonarData();
00075
00076
00077 private: double updatePeriod, updateTime;
00078
00079
00080 private: double wheelSep, wheelDiam;
00081
00082
00083 private: double sonarMaxRange;
00084 private: GzVector pos[16];
00085 private: double dir[16];
00086
00087
00088 private: Body *chassis;
00089 private: Body *wheels[2];
00090 private: HingeJoint *wheelJoints[3];
00091 private: Body *castorWheel;
00092 private: BallJoint *castorJoint;
00093
00094
00095 private: RayProximity *sonarSensor;
00096
00097
00098 private: gz_position_t *position_iface;
00099 private: gz_power_t *power_iface;
00100 private: gz_sonar_t *sonar_iface;
00101
00102
00103 private: int enableMotors;
00104 private: double wheelSpeed[2];
00105
00106
00107 private: double odomPose[3];
00108
00109
00110 private: double odomVel[3];
00111
00112
00113 private: double batteryLevel;
00114
00115
00116 private: double batteryCurve[2];
00117 };
00118
00119 #endif