QueryTrajectoryState.h

00001 #ifndef _ROS_SERVICE_QueryTrajectoryState_h
00002 #define _ROS_SERVICE_QueryTrajectoryState_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "ros/time.h"
00008 
00009 namespace control_msgs
00010 {
00011 
00012 static const char QUERYTRAJECTORYSTATE[] = "control_msgs/QueryTrajectoryState";
00013 
00014   class QueryTrajectoryStateRequest : public ros::Msg
00015   {
00016     public:
00017       ros::Time time;
00018 
00019     QueryTrajectoryStateRequest():
00020       time()
00021     {
00022     }
00023 
00024     virtual int serialize(unsigned char *outbuffer) const
00025     {
00026       int offset = 0;
00027       *(outbuffer + offset + 0) = (this->time.sec >> (8 * 0)) & 0xFF;
00028       *(outbuffer + offset + 1) = (this->time.sec >> (8 * 1)) & 0xFF;
00029       *(outbuffer + offset + 2) = (this->time.sec >> (8 * 2)) & 0xFF;
00030       *(outbuffer + offset + 3) = (this->time.sec >> (8 * 3)) & 0xFF;
00031       offset += sizeof(this->time.sec);
00032       *(outbuffer + offset + 0) = (this->time.nsec >> (8 * 0)) & 0xFF;
00033       *(outbuffer + offset + 1) = (this->time.nsec >> (8 * 1)) & 0xFF;
00034       *(outbuffer + offset + 2) = (this->time.nsec >> (8 * 2)) & 0xFF;
00035       *(outbuffer + offset + 3) = (this->time.nsec >> (8 * 3)) & 0xFF;
00036       offset += sizeof(this->time.nsec);
00037       return offset;
00038     }
00039 
00040     virtual int deserialize(unsigned char *inbuffer)
00041     {
00042       int offset = 0;
00043       this->time.sec =  ((uint32_t) (*(inbuffer + offset)));
00044       this->time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00045       this->time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00046       this->time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00047       offset += sizeof(this->time.sec);
00048       this->time.nsec =  ((uint32_t) (*(inbuffer + offset)));
00049       this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00050       this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00051       this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00052       offset += sizeof(this->time.nsec);
00053      return offset;
00054     }
00055 
00056     const char * getType(){ return QUERYTRAJECTORYSTATE; };
00057     const char * getMD5(){ return "556a4fb76023a469987922359d08a844"; };
00058 
00059   };
00060 
00061   class QueryTrajectoryStateResponse : public ros::Msg
00062   {
00063     public:
00064       uint8_t name_length;
00065       char* st_name;
00066       char* * name;
00067       uint8_t position_length;
00068       double st_position;
00069       double * position;
00070       uint8_t velocity_length;
00071       double st_velocity;
00072       double * velocity;
00073       uint8_t acceleration_length;
00074       double st_acceleration;
00075       double * acceleration;
00076 
00077     QueryTrajectoryStateResponse():
00078       name_length(0), name(NULL),
00079       position_length(0), position(NULL),
00080       velocity_length(0), velocity(NULL),
00081       acceleration_length(0), acceleration(NULL)
00082     {
00083     }
00084 
00085     virtual int serialize(unsigned char *outbuffer) const
00086     {
00087       int offset = 0;
00088       *(outbuffer + offset++) = name_length;
00089       *(outbuffer + offset++) = 0;
00090       *(outbuffer + offset++) = 0;
00091       *(outbuffer + offset++) = 0;
00092       for( uint8_t i = 0; i < name_length; i++){
00093       uint32_t length_namei = strlen(this->name[i]);
00094       memcpy(outbuffer + offset, &length_namei, sizeof(uint32_t));
00095       offset += 4;
00096       memcpy(outbuffer + offset, this->name[i], length_namei);
00097       offset += length_namei;
00098       }
00099       *(outbuffer + offset++) = position_length;
00100       *(outbuffer + offset++) = 0;
00101       *(outbuffer + offset++) = 0;
00102       *(outbuffer + offset++) = 0;
00103       for( uint8_t i = 0; i < position_length; i++){
00104       union {
00105         double real;
00106         uint64_t base;
00107       } u_positioni;
00108       u_positioni.real = this->position[i];
00109       *(outbuffer + offset + 0) = (u_positioni.base >> (8 * 0)) & 0xFF;
00110       *(outbuffer + offset + 1) = (u_positioni.base >> (8 * 1)) & 0xFF;
00111       *(outbuffer + offset + 2) = (u_positioni.base >> (8 * 2)) & 0xFF;
00112       *(outbuffer + offset + 3) = (u_positioni.base >> (8 * 3)) & 0xFF;
00113       *(outbuffer + offset + 4) = (u_positioni.base >> (8 * 4)) & 0xFF;
00114       *(outbuffer + offset + 5) = (u_positioni.base >> (8 * 5)) & 0xFF;
00115       *(outbuffer + offset + 6) = (u_positioni.base >> (8 * 6)) & 0xFF;
00116       *(outbuffer + offset + 7) = (u_positioni.base >> (8 * 7)) & 0xFF;
00117       offset += sizeof(this->position[i]);
00118       }
00119       *(outbuffer + offset++) = velocity_length;
00120       *(outbuffer + offset++) = 0;
00121       *(outbuffer + offset++) = 0;
00122       *(outbuffer + offset++) = 0;
00123       for( uint8_t i = 0; i < velocity_length; i++){
00124       union {
00125         double real;
00126         uint64_t base;
00127       } u_velocityi;
00128       u_velocityi.real = this->velocity[i];
00129       *(outbuffer + offset + 0) = (u_velocityi.base >> (8 * 0)) & 0xFF;
00130       *(outbuffer + offset + 1) = (u_velocityi.base >> (8 * 1)) & 0xFF;
00131       *(outbuffer + offset + 2) = (u_velocityi.base >> (8 * 2)) & 0xFF;
00132       *(outbuffer + offset + 3) = (u_velocityi.base >> (8 * 3)) & 0xFF;
00133       *(outbuffer + offset + 4) = (u_velocityi.base >> (8 * 4)) & 0xFF;
00134       *(outbuffer + offset + 5) = (u_velocityi.base >> (8 * 5)) & 0xFF;
00135       *(outbuffer + offset + 6) = (u_velocityi.base >> (8 * 6)) & 0xFF;
00136       *(outbuffer + offset + 7) = (u_velocityi.base >> (8 * 7)) & 0xFF;
00137       offset += sizeof(this->velocity[i]);
00138       }
00139       *(outbuffer + offset++) = acceleration_length;
00140       *(outbuffer + offset++) = 0;
00141       *(outbuffer + offset++) = 0;
00142       *(outbuffer + offset++) = 0;
00143       for( uint8_t i = 0; i < acceleration_length; i++){
00144       union {
00145         double real;
00146         uint64_t base;
00147       } u_accelerationi;
00148       u_accelerationi.real = this->acceleration[i];
00149       *(outbuffer + offset + 0) = (u_accelerationi.base >> (8 * 0)) & 0xFF;
00150       *(outbuffer + offset + 1) = (u_accelerationi.base >> (8 * 1)) & 0xFF;
00151       *(outbuffer + offset + 2) = (u_accelerationi.base >> (8 * 2)) & 0xFF;
00152       *(outbuffer + offset + 3) = (u_accelerationi.base >> (8 * 3)) & 0xFF;
00153       *(outbuffer + offset + 4) = (u_accelerationi.base >> (8 * 4)) & 0xFF;
00154       *(outbuffer + offset + 5) = (u_accelerationi.base >> (8 * 5)) & 0xFF;
00155       *(outbuffer + offset + 6) = (u_accelerationi.base >> (8 * 6)) & 0xFF;
00156       *(outbuffer + offset + 7) = (u_accelerationi.base >> (8 * 7)) & 0xFF;
00157       offset += sizeof(this->acceleration[i]);
00158       }
00159       return offset;
00160     }
00161 
00162     virtual int deserialize(unsigned char *inbuffer)
00163     {
00164       int offset = 0;
00165       uint8_t name_lengthT = *(inbuffer + offset++);
00166       if(name_lengthT > name_length)
00167         this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*));
00168       offset += 3;
00169       name_length = name_lengthT;
00170       for( uint8_t i = 0; i < name_length; i++){
00171       uint32_t length_st_name;
00172       memcpy(&length_st_name, (inbuffer + offset), sizeof(uint32_t));
00173       offset += 4;
00174       for(unsigned int k= offset; k< offset+length_st_name; ++k){
00175           inbuffer[k-1]=inbuffer[k];
00176       }
00177       inbuffer[offset+length_st_name-1]=0;
00178       this->st_name = (char *)(inbuffer + offset-1);
00179       offset += length_st_name;
00180         memcpy( &(this->name[i]), &(this->st_name), sizeof(char*));
00181       }
00182       uint8_t position_lengthT = *(inbuffer + offset++);
00183       if(position_lengthT > position_length)
00184         this->position = (double*)realloc(this->position, position_lengthT * sizeof(double));
00185       offset += 3;
00186       position_length = position_lengthT;
00187       for( uint8_t i = 0; i < position_length; i++){
00188       union {
00189         double real;
00190         uint64_t base;
00191       } u_st_position;
00192       u_st_position.base = 0;
00193       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00194       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00195       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00196       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00197       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00198       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00199       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00200       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00201       this->st_position = u_st_position.real;
00202       offset += sizeof(this->st_position);
00203         memcpy( &(this->position[i]), &(this->st_position), sizeof(double));
00204       }
00205       uint8_t velocity_lengthT = *(inbuffer + offset++);
00206       if(velocity_lengthT > velocity_length)
00207         this->velocity = (double*)realloc(this->velocity, velocity_lengthT * sizeof(double));
00208       offset += 3;
00209       velocity_length = velocity_lengthT;
00210       for( uint8_t i = 0; i < velocity_length; i++){
00211       union {
00212         double real;
00213         uint64_t base;
00214       } u_st_velocity;
00215       u_st_velocity.base = 0;
00216       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00217       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00218       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00219       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00220       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00221       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00222       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00223       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00224       this->st_velocity = u_st_velocity.real;
00225       offset += sizeof(this->st_velocity);
00226         memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(double));
00227       }
00228       uint8_t acceleration_lengthT = *(inbuffer + offset++);
00229       if(acceleration_lengthT > acceleration_length)
00230         this->acceleration = (double*)realloc(this->acceleration, acceleration_lengthT * sizeof(double));
00231       offset += 3;
00232       acceleration_length = acceleration_lengthT;
00233       for( uint8_t i = 0; i < acceleration_length; i++){
00234       union {
00235         double real;
00236         uint64_t base;
00237       } u_st_acceleration;
00238       u_st_acceleration.base = 0;
00239       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00240       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00241       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00242       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00243       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00244       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00245       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00246       u_st_acceleration.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00247       this->st_acceleration = u_st_acceleration.real;
00248       offset += sizeof(this->st_acceleration);
00249         memcpy( &(this->acceleration[i]), &(this->st_acceleration), sizeof(double));
00250       }
00251      return offset;
00252     }
00253 
00254     const char * getType(){ return QUERYTRAJECTORYSTATE; };
00255     const char * getMD5(){ return "1f1a6554ad060f44d013e71868403c1a"; };
00256 
00257   };
00258 
00259   class QueryTrajectoryState {
00260     public:
00261     typedef QueryTrajectoryStateRequest Request;
00262     typedef QueryTrajectoryStateResponse Response;
00263   };
00264 
00265 }
00266 #endif