PursuingMessages.h
Go to the documentation of this file.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
00026 #ifndef __PURSUING_MESSAGES__
00027 #define __PURSUING_MESSAGES__
00028
00029 #define PURSUING_MSG_ID "pursuing"
00030 #define PURSUING_CENTRAL_ID 100
00031
00032 #include <Types.h>
00033 #include <string>
00034 #include <vector>
00035
00036 #include <Message.h>
00037
00038 #define MAX_SCAN_SIZE 1024
00039
00040 using namespace std;
00041 using namespace MipBaselib;
00042
00045 enum PursuingHeaders {
00046
00047 PUR_HEADER_GO_TO_POSE,
00048 PUR_HEADER_REQ_SCAN,
00049 PUR_HEADER_INITIALIZE,
00050
00051
00052 PUR_HEADER_POSE_AND_SCAN,
00053 PUR_HEADER_POSE,
00054 PUR_HEADER_PARAMS,
00055
00056 PUR_HEADERS_NUM,
00057 };
00058
00059 static const char* pursuingHeaderNames[PUR_HEADERS_NUM] = {
00060
00061 "go to pose",
00062 "request scan",
00063 "initialize",
00064
00065
00066 "send pose and scan",
00067 "send pose",
00068 "send parameters",
00069 };
00070
00071
00072 class PursuingMsg : public MipBaselib::Message{
00073 protected:
00074 PursuingMsg(){
00075 string id(PURSUING_MSG_ID);
00076 setId(id);
00077 }
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087 public:
00088 int robotId;
00089 PursuingHeaders header;
00090
00091 PursuingMsg(const string& inMessage){
00092 inMessage.copy((char* ) this, sizeof(PursuingMsg));
00093 }
00094 };
00095
00096
00097
00098
00099 class PursuingMsgGoToPose : public PursuingMsg{
00100 public:
00101
00102 double x;
00103 double y;
00104 double theta;
00105 bool bReportScan;
00106
00110 PursuingMsgGoToPose(int robotId_, double x_,
00111 double y_,
00112 double theta_,
00113 bool bReportScan_){
00114 robotId = robotId_;
00115 header = PUR_HEADER_GO_TO_POSE;
00116 x = x_;
00117 y = y_;
00118 theta = theta_;
00119 bReportScan = bReportScan_;
00120 }
00121
00123 PursuingMsgGoToPose(const string& inMessage){
00124 constructor<PursuingMsgGoToPose>(inMessage);
00125 }
00126
00129 void encode(string& outMessage){
00130 encoder<PursuingMsgGoToPose>(outMessage);
00131
00132 }
00133 };
00134
00135 class PursuingMsgReqScan : public PursuingMsg{
00136 public:
00137
00138 PursuingMsgReqScan(int robotId_){
00139 robotId = robotId_;
00140 header = PUR_HEADER_REQ_SCAN;
00141 }
00142
00144 PursuingMsgReqScan(const string& inMessage){
00145 constructor<PursuingMsgReqScan>(inMessage);
00146 }
00147
00150 void encode(string& outMessage){
00151 encoder<PursuingMsgReqScan>(outMessage);
00152 }
00153 };
00154
00155 class PursuingMsgInitialize : public PursuingMsg{
00156 public:
00157
00158 PursuingMsgInitialize(int robotId_){
00159 robotId = robotId_;
00160 header = PUR_HEADER_INITIALIZE;
00161 }
00162
00164 PursuingMsgInitialize(const string& inMessage){
00165 constructor<PursuingMsgInitialize>(inMessage);
00166 }
00167
00170 void encode(string& outMessage){
00171 encoder<PursuingMsgInitialize>(outMessage);
00172 }
00173 };
00174
00175
00176
00177 class PursuingMsgPoseAndScan : public PursuingMsg{
00178 public:
00179
00180 double x;
00181 double y;
00182 double theta;
00183 double scanLengthArray[MAX_SCAN_SIZE];
00184 double scanBearingArray[MAX_SCAN_SIZE];
00185 int scanSize;
00186
00187 PursuingMsgPoseAndScan(int robotId_, double x_,
00188 double y_,
00189 double theta_,
00190 Scan &scan){
00191 robotId = robotId_;
00192 header = PUR_HEADER_POSE_AND_SCAN;
00193 x = x_;
00194 y = y_;
00195 theta = theta_;
00196 while( !scan.askExclusiveAccess(Time(1,0)) ) { usleep(1000); };
00197 while( !scan.getSize(scanSize) ){ usleep(1000); };
00198 scanSize = min( MAX_SCAN_SIZE, scanSize );
00199
00200 cout<< " sending scan size " << scanSize << endl;
00201
00202 Ray ray;
00203 for( int i = 0; i < scanSize; i++ )
00204 {
00205 while( !scan.getRay( ray, i ) ) { usleep(1000); };
00206
00207 scanLengthArray[i] = ray.reading();
00208 scanBearingArray[i] = ray.bearing().dCast2Pi();
00209
00210 if( i % 100 == 0 ) cout<< " " << robotId << ", ray " << i << " " << ray.reading() << "," << ray.bearing().print() << endl;
00211 }
00212 scan.leaveExclusiveAccess();
00213 }
00214
00216 PursuingMsgPoseAndScan(const string& inMessage){
00217 constructor<PursuingMsgPoseAndScan>(inMessage);
00218 }
00219
00222 void encode(string& outMessage){
00223 encoder<PursuingMsgPoseAndScan>(outMessage);
00224 }
00225 };
00226
00227
00228 class PursuingMsgPose : public PursuingMsg{
00229 public:
00230
00231 double x;
00232 double y;
00233 double theta;
00234
00235 PursuingMsgPose(int robotId_, double x_,
00236 double y_,
00237 double theta_){
00238 robotId = robotId_;
00239 header = PUR_HEADER_POSE;
00240 x = x_;
00241 y = y_;
00242 theta = theta_;
00243 }
00244
00246 PursuingMsgPose(const string& inMessage){
00247 constructor<PursuingMsgPose>(inMessage);
00248 }
00249
00252 void encode(string& outMessage){
00253 encoder<PursuingMsgPose>(outMessage);
00254 }
00255 };
00256
00257 class PursuingMsgParams : public PursuingMsg{
00258 public:
00259
00260 double sensorLinMaxRange;
00261 double sensorFieldOfView;
00262 int sensorCount;
00263 double sensorResolution;
00264
00265 double robotRadius;
00266
00267 double x;
00268 double y;
00269 double theta;
00270
00271 PursuingMsgParams(int robotId_, double sensorLinMaxRange_,
00272 double sensorFieldOfView_,
00273 int sensorCount_,
00274 double sensorResolution_,
00275 double robotRadius_,
00276 double x_,
00277 double y_,
00278 double theta_)
00279 {
00280 robotId = robotId_;
00281 header = PUR_HEADER_PARAMS;
00282 sensorLinMaxRange = sensorLinMaxRange_;
00283 sensorFieldOfView = sensorFieldOfView_;
00284 sensorCount = sensorCount_;
00285 sensorResolution = sensorResolution_;
00286
00287 robotRadius = robotRadius_;
00288
00289 x = x_;
00290 y = y_;
00291 theta = theta_;
00292 }
00293
00295 PursuingMsgParams(const string& inMessage){
00296 constructor<PursuingMsgParams>(inMessage);
00297 }
00298
00301 void encode(string& outMessage){
00302 encoder<PursuingMsgParams>(outMessage);
00303 }
00304 };
00305
00306
00307 #endif
00308
00309