Go to the documentation of this file.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 virtual int serialize(unsigned char *outbuffer) const
00020 {
00021 int offset = 0;
00022 *(outbuffer + offset + 0) = (this->time.sec >> (8 * 0)) & 0xFF;
00023 *(outbuffer + offset + 1) = (this->time.sec >> (8 * 1)) & 0xFF;
00024 *(outbuffer + offset + 2) = (this->time.sec >> (8 * 2)) & 0xFF;
00025 *(outbuffer + offset + 3) = (this->time.sec >> (8 * 3)) & 0xFF;
00026 offset += sizeof(this->time.sec);
00027 *(outbuffer + offset + 0) = (this->time.nsec >> (8 * 0)) & 0xFF;
00028 *(outbuffer + offset + 1) = (this->time.nsec >> (8 * 1)) & 0xFF;
00029 *(outbuffer + offset + 2) = (this->time.nsec >> (8 * 2)) & 0xFF;
00030 *(outbuffer + offset + 3) = (this->time.nsec >> (8 * 3)) & 0xFF;
00031 offset += sizeof(this->time.nsec);
00032 return offset;
00033 }
00034
00035 virtual int deserialize(unsigned char *inbuffer)
00036 {
00037 int offset = 0;
00038 this->time.sec = ((uint32_t) (*(inbuffer + offset)));
00039 this->time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00040 this->time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00041 this->time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00042 offset += sizeof(this->time.sec);
00043 this->time.nsec = ((uint32_t) (*(inbuffer + offset)));
00044 this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00045 this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00046 this->time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00047 offset += sizeof(this->time.nsec);
00048 return offset;
00049 }
00050
00051 const char * getType(){ return QUERYTRAJECTORYSTATE; };
00052 const char * getMD5(){ return "556a4fb76023a469987922359d08a844"; };
00053
00054 };
00055
00056 class QueryTrajectoryStateResponse : public ros::Msg
00057 {
00058 public:
00059 uint8_t name_length;
00060 char* st_name;
00061 char* * name;
00062 uint8_t position_length;
00063 float st_position;
00064 float * position;
00065 uint8_t velocity_length;
00066 float st_velocity;
00067 float * velocity;
00068 uint8_t acceleration_length;
00069 float st_acceleration;
00070 float * acceleration;
00071
00072 virtual int serialize(unsigned char *outbuffer) const
00073 {
00074 int offset = 0;
00075 *(outbuffer + offset++) = name_length;
00076 *(outbuffer + offset++) = 0;
00077 *(outbuffer + offset++) = 0;
00078 *(outbuffer + offset++) = 0;
00079 for( uint8_t i = 0; i < name_length; i++){
00080 uint32_t length_namei = strlen(this->name[i]);
00081 memcpy(outbuffer + offset, &length_namei, sizeof(uint32_t));
00082 offset += 4;
00083 memcpy(outbuffer + offset, this->name[i], length_namei);
00084 offset += length_namei;
00085 }
00086 *(outbuffer + offset++) = position_length;
00087 *(outbuffer + offset++) = 0;
00088 *(outbuffer + offset++) = 0;
00089 *(outbuffer + offset++) = 0;
00090 for( uint8_t i = 0; i < position_length; i++){
00091 int32_t * val_positioni = (int32_t *) &(this->position[i]);
00092 int32_t exp_positioni = (((*val_positioni)>>23)&255);
00093 if(exp_positioni != 0)
00094 exp_positioni += 1023-127;
00095 int32_t sig_positioni = *val_positioni;
00096 *(outbuffer + offset++) = 0;
00097 *(outbuffer + offset++) = 0;
00098 *(outbuffer + offset++) = 0;
00099 *(outbuffer + offset++) = (sig_positioni<<5) & 0xff;
00100 *(outbuffer + offset++) = (sig_positioni>>3) & 0xff;
00101 *(outbuffer + offset++) = (sig_positioni>>11) & 0xff;
00102 *(outbuffer + offset++) = ((exp_positioni<<4) & 0xF0) | ((sig_positioni>>19)&0x0F);
00103 *(outbuffer + offset++) = (exp_positioni>>4) & 0x7F;
00104 if(this->position[i] < 0) *(outbuffer + offset -1) |= 0x80;
00105 }
00106 *(outbuffer + offset++) = velocity_length;
00107 *(outbuffer + offset++) = 0;
00108 *(outbuffer + offset++) = 0;
00109 *(outbuffer + offset++) = 0;
00110 for( uint8_t i = 0; i < velocity_length; i++){
00111 int32_t * val_velocityi = (int32_t *) &(this->velocity[i]);
00112 int32_t exp_velocityi = (((*val_velocityi)>>23)&255);
00113 if(exp_velocityi != 0)
00114 exp_velocityi += 1023-127;
00115 int32_t sig_velocityi = *val_velocityi;
00116 *(outbuffer + offset++) = 0;
00117 *(outbuffer + offset++) = 0;
00118 *(outbuffer + offset++) = 0;
00119 *(outbuffer + offset++) = (sig_velocityi<<5) & 0xff;
00120 *(outbuffer + offset++) = (sig_velocityi>>3) & 0xff;
00121 *(outbuffer + offset++) = (sig_velocityi>>11) & 0xff;
00122 *(outbuffer + offset++) = ((exp_velocityi<<4) & 0xF0) | ((sig_velocityi>>19)&0x0F);
00123 *(outbuffer + offset++) = (exp_velocityi>>4) & 0x7F;
00124 if(this->velocity[i] < 0) *(outbuffer + offset -1) |= 0x80;
00125 }
00126 *(outbuffer + offset++) = acceleration_length;
00127 *(outbuffer + offset++) = 0;
00128 *(outbuffer + offset++) = 0;
00129 *(outbuffer + offset++) = 0;
00130 for( uint8_t i = 0; i < acceleration_length; i++){
00131 int32_t * val_accelerationi = (int32_t *) &(this->acceleration[i]);
00132 int32_t exp_accelerationi = (((*val_accelerationi)>>23)&255);
00133 if(exp_accelerationi != 0)
00134 exp_accelerationi += 1023-127;
00135 int32_t sig_accelerationi = *val_accelerationi;
00136 *(outbuffer + offset++) = 0;
00137 *(outbuffer + offset++) = 0;
00138 *(outbuffer + offset++) = 0;
00139 *(outbuffer + offset++) = (sig_accelerationi<<5) & 0xff;
00140 *(outbuffer + offset++) = (sig_accelerationi>>3) & 0xff;
00141 *(outbuffer + offset++) = (sig_accelerationi>>11) & 0xff;
00142 *(outbuffer + offset++) = ((exp_accelerationi<<4) & 0xF0) | ((sig_accelerationi>>19)&0x0F);
00143 *(outbuffer + offset++) = (exp_accelerationi>>4) & 0x7F;
00144 if(this->acceleration[i] < 0) *(outbuffer + offset -1) |= 0x80;
00145 }
00146 return offset;
00147 }
00148
00149 virtual int deserialize(unsigned char *inbuffer)
00150 {
00151 int offset = 0;
00152 uint8_t name_lengthT = *(inbuffer + offset++);
00153 if(name_lengthT > name_length)
00154 this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*));
00155 offset += 3;
00156 name_length = name_lengthT;
00157 for( uint8_t i = 0; i < name_length; i++){
00158 uint32_t length_st_name;
00159 memcpy(&length_st_name, (inbuffer + offset), sizeof(uint32_t));
00160 offset += 4;
00161 for(unsigned int k= offset; k< offset+length_st_name; ++k){
00162 inbuffer[k-1]=inbuffer[k];
00163 }
00164 inbuffer[offset+length_st_name-1]=0;
00165 this->st_name = (char *)(inbuffer + offset-1);
00166 offset += length_st_name;
00167 memcpy( &(this->name[i]), &(this->st_name), sizeof(char*));
00168 }
00169 uint8_t position_lengthT = *(inbuffer + offset++);
00170 if(position_lengthT > position_length)
00171 this->position = (float*)realloc(this->position, position_lengthT * sizeof(float));
00172 offset += 3;
00173 position_length = position_lengthT;
00174 for( uint8_t i = 0; i < position_length; i++){
00175 uint32_t * val_st_position = (uint32_t*) &(this->st_position);
00176 offset += 3;
00177 *val_st_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00178 *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00179 *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00180 *val_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00181 uint32_t exp_st_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00182 exp_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00183 if(exp_st_position !=0)
00184 *val_st_position |= ((exp_st_position)-1023+127)<<23;
00185 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_position = -this->st_position;
00186 memcpy( &(this->position[i]), &(this->st_position), sizeof(float));
00187 }
00188 uint8_t velocity_lengthT = *(inbuffer + offset++);
00189 if(velocity_lengthT > velocity_length)
00190 this->velocity = (float*)realloc(this->velocity, velocity_lengthT * sizeof(float));
00191 offset += 3;
00192 velocity_length = velocity_lengthT;
00193 for( uint8_t i = 0; i < velocity_length; i++){
00194 uint32_t * val_st_velocity = (uint32_t*) &(this->st_velocity);
00195 offset += 3;
00196 *val_st_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00197 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00198 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00199 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00200 uint32_t exp_st_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00201 exp_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00202 if(exp_st_velocity !=0)
00203 *val_st_velocity |= ((exp_st_velocity)-1023+127)<<23;
00204 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_velocity = -this->st_velocity;
00205 memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(float));
00206 }
00207 uint8_t acceleration_lengthT = *(inbuffer + offset++);
00208 if(acceleration_lengthT > acceleration_length)
00209 this->acceleration = (float*)realloc(this->acceleration, acceleration_lengthT * sizeof(float));
00210 offset += 3;
00211 acceleration_length = acceleration_lengthT;
00212 for( uint8_t i = 0; i < acceleration_length; i++){
00213 uint32_t * val_st_acceleration = (uint32_t*) &(this->st_acceleration);
00214 offset += 3;
00215 *val_st_acceleration = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00216 *val_st_acceleration |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00217 *val_st_acceleration |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00218 *val_st_acceleration |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00219 uint32_t exp_st_acceleration = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00220 exp_st_acceleration |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00221 if(exp_st_acceleration !=0)
00222 *val_st_acceleration |= ((exp_st_acceleration)-1023+127)<<23;
00223 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_acceleration = -this->st_acceleration;
00224 memcpy( &(this->acceleration[i]), &(this->st_acceleration), sizeof(float));
00225 }
00226 return offset;
00227 }
00228
00229 const char * getType(){ return QUERYTRAJECTORYSTATE; };
00230 const char * getMD5(){ return "1f1a6554ad060f44d013e71868403c1a"; };
00231
00232 };
00233
00234 class QueryTrajectoryState {
00235 public:
00236 typedef QueryTrajectoryStateRequest Request;
00237 typedef QueryTrajectoryStateResponse Response;
00238 };
00239
00240 }
00241 #endif