JointState.h

00001 #ifndef _ROS_sensor_msgs_JointState_h
00002 #define _ROS_sensor_msgs_JointState_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "std_msgs/Header.h"
00009 
00010 namespace sensor_msgs
00011 {
00012 
00013   class JointState : public ros::Msg
00014   {
00015     public:
00016       std_msgs::Header header;
00017       uint8_t name_length;
00018       char* st_name;
00019       char* * name;
00020       uint8_t position_length;
00021       double st_position;
00022       double * position;
00023       uint8_t velocity_length;
00024       double st_velocity;
00025       double * velocity;
00026       uint8_t effort_length;
00027       double st_effort;
00028       double * effort;
00029 
00030     JointState():
00031       header(),
00032       name_length(0), name(NULL),
00033       position_length(0), position(NULL),
00034       velocity_length(0), velocity(NULL),
00035       effort_length(0), effort(NULL)
00036     {
00037     }
00038 
00039     virtual int serialize(unsigned char *outbuffer) const
00040     {
00041       int offset = 0;
00042       offset += this->header.serialize(outbuffer + offset);
00043       *(outbuffer + offset++) = name_length;
00044       *(outbuffer + offset++) = 0;
00045       *(outbuffer + offset++) = 0;
00046       *(outbuffer + offset++) = 0;
00047       for( uint8_t i = 0; i < name_length; i++){
00048       uint32_t length_namei = strlen(this->name[i]);
00049       memcpy(outbuffer + offset, &length_namei, sizeof(uint32_t));
00050       offset += 4;
00051       memcpy(outbuffer + offset, this->name[i], length_namei);
00052       offset += length_namei;
00053       }
00054       *(outbuffer + offset++) = position_length;
00055       *(outbuffer + offset++) = 0;
00056       *(outbuffer + offset++) = 0;
00057       *(outbuffer + offset++) = 0;
00058       for( uint8_t i = 0; i < position_length; i++){
00059       union {
00060         double real;
00061         uint64_t base;
00062       } u_positioni;
00063       u_positioni.real = this->position[i];
00064       *(outbuffer + offset + 0) = (u_positioni.base >> (8 * 0)) & 0xFF;
00065       *(outbuffer + offset + 1) = (u_positioni.base >> (8 * 1)) & 0xFF;
00066       *(outbuffer + offset + 2) = (u_positioni.base >> (8 * 2)) & 0xFF;
00067       *(outbuffer + offset + 3) = (u_positioni.base >> (8 * 3)) & 0xFF;
00068       *(outbuffer + offset + 4) = (u_positioni.base >> (8 * 4)) & 0xFF;
00069       *(outbuffer + offset + 5) = (u_positioni.base >> (8 * 5)) & 0xFF;
00070       *(outbuffer + offset + 6) = (u_positioni.base >> (8 * 6)) & 0xFF;
00071       *(outbuffer + offset + 7) = (u_positioni.base >> (8 * 7)) & 0xFF;
00072       offset += sizeof(this->position[i]);
00073       }
00074       *(outbuffer + offset++) = velocity_length;
00075       *(outbuffer + offset++) = 0;
00076       *(outbuffer + offset++) = 0;
00077       *(outbuffer + offset++) = 0;
00078       for( uint8_t i = 0; i < velocity_length; i++){
00079       union {
00080         double real;
00081         uint64_t base;
00082       } u_velocityi;
00083       u_velocityi.real = this->velocity[i];
00084       *(outbuffer + offset + 0) = (u_velocityi.base >> (8 * 0)) & 0xFF;
00085       *(outbuffer + offset + 1) = (u_velocityi.base >> (8 * 1)) & 0xFF;
00086       *(outbuffer + offset + 2) = (u_velocityi.base >> (8 * 2)) & 0xFF;
00087       *(outbuffer + offset + 3) = (u_velocityi.base >> (8 * 3)) & 0xFF;
00088       *(outbuffer + offset + 4) = (u_velocityi.base >> (8 * 4)) & 0xFF;
00089       *(outbuffer + offset + 5) = (u_velocityi.base >> (8 * 5)) & 0xFF;
00090       *(outbuffer + offset + 6) = (u_velocityi.base >> (8 * 6)) & 0xFF;
00091       *(outbuffer + offset + 7) = (u_velocityi.base >> (8 * 7)) & 0xFF;
00092       offset += sizeof(this->velocity[i]);
00093       }
00094       *(outbuffer + offset++) = effort_length;
00095       *(outbuffer + offset++) = 0;
00096       *(outbuffer + offset++) = 0;
00097       *(outbuffer + offset++) = 0;
00098       for( uint8_t i = 0; i < effort_length; i++){
00099       union {
00100         double real;
00101         uint64_t base;
00102       } u_efforti;
00103       u_efforti.real = this->effort[i];
00104       *(outbuffer + offset + 0) = (u_efforti.base >> (8 * 0)) & 0xFF;
00105       *(outbuffer + offset + 1) = (u_efforti.base >> (8 * 1)) & 0xFF;
00106       *(outbuffer + offset + 2) = (u_efforti.base >> (8 * 2)) & 0xFF;
00107       *(outbuffer + offset + 3) = (u_efforti.base >> (8 * 3)) & 0xFF;
00108       *(outbuffer + offset + 4) = (u_efforti.base >> (8 * 4)) & 0xFF;
00109       *(outbuffer + offset + 5) = (u_efforti.base >> (8 * 5)) & 0xFF;
00110       *(outbuffer + offset + 6) = (u_efforti.base >> (8 * 6)) & 0xFF;
00111       *(outbuffer + offset + 7) = (u_efforti.base >> (8 * 7)) & 0xFF;
00112       offset += sizeof(this->effort[i]);
00113       }
00114       return offset;
00115     }
00116 
00117     virtual int deserialize(unsigned char *inbuffer)
00118     {
00119       int offset = 0;
00120       offset += this->header.deserialize(inbuffer + offset);
00121       uint8_t name_lengthT = *(inbuffer + offset++);
00122       if(name_lengthT > name_length)
00123         this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*));
00124       offset += 3;
00125       name_length = name_lengthT;
00126       for( uint8_t i = 0; i < name_length; i++){
00127       uint32_t length_st_name;
00128       memcpy(&length_st_name, (inbuffer + offset), sizeof(uint32_t));
00129       offset += 4;
00130       for(unsigned int k= offset; k< offset+length_st_name; ++k){
00131           inbuffer[k-1]=inbuffer[k];
00132       }
00133       inbuffer[offset+length_st_name-1]=0;
00134       this->st_name = (char *)(inbuffer + offset-1);
00135       offset += length_st_name;
00136         memcpy( &(this->name[i]), &(this->st_name), sizeof(char*));
00137       }
00138       uint8_t position_lengthT = *(inbuffer + offset++);
00139       if(position_lengthT > position_length)
00140         this->position = (double*)realloc(this->position, position_lengthT * sizeof(double));
00141       offset += 3;
00142       position_length = position_lengthT;
00143       for( uint8_t i = 0; i < position_length; i++){
00144       union {
00145         double real;
00146         uint64_t base;
00147       } u_st_position;
00148       u_st_position.base = 0;
00149       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00150       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00151       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00152       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00153       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00154       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00155       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00156       u_st_position.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00157       this->st_position = u_st_position.real;
00158       offset += sizeof(this->st_position);
00159         memcpy( &(this->position[i]), &(this->st_position), sizeof(double));
00160       }
00161       uint8_t velocity_lengthT = *(inbuffer + offset++);
00162       if(velocity_lengthT > velocity_length)
00163         this->velocity = (double*)realloc(this->velocity, velocity_lengthT * sizeof(double));
00164       offset += 3;
00165       velocity_length = velocity_lengthT;
00166       for( uint8_t i = 0; i < velocity_length; i++){
00167       union {
00168         double real;
00169         uint64_t base;
00170       } u_st_velocity;
00171       u_st_velocity.base = 0;
00172       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00173       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00174       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00175       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00176       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00177       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00178       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00179       u_st_velocity.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00180       this->st_velocity = u_st_velocity.real;
00181       offset += sizeof(this->st_velocity);
00182         memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(double));
00183       }
00184       uint8_t effort_lengthT = *(inbuffer + offset++);
00185       if(effort_lengthT > effort_length)
00186         this->effort = (double*)realloc(this->effort, effort_lengthT * sizeof(double));
00187       offset += 3;
00188       effort_length = effort_lengthT;
00189       for( uint8_t i = 0; i < effort_length; i++){
00190       union {
00191         double real;
00192         uint64_t base;
00193       } u_st_effort;
00194       u_st_effort.base = 0;
00195       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00196       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00197       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00198       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00199       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00200       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00201       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00202       u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00203       this->st_effort = u_st_effort.real;
00204       offset += sizeof(this->st_effort);
00205         memcpy( &(this->effort[i]), &(this->st_effort), sizeof(double));
00206       }
00207      return offset;
00208     }
00209 
00210     const char * getType(){ return "sensor_msgs/JointState"; };
00211     const char * getMD5(){ return "3066dcd76a6cfaef579bd0f34173e9fd"; };
00212 
00213   };
00214 
00215 }
00216 #endif