QueryTrajectoryState.h
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


ric_mc
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:39:50