JointState.h
Go to the documentation of this file.
00001 #ifndef _ROS_dynamixel_msgs_JointState_h
00002 #define _ROS_dynamixel_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 dynamixel_msgs
00011 {
00012 
00013   class JointState : public ros::Msg
00014   {
00015     public:
00016       std_msgs::Header header;
00017       const char* name;
00018       uint8_t motor_ids_length;
00019       int32_t st_motor_ids;
00020       int32_t * motor_ids;
00021       uint8_t motor_temps_length;
00022       int32_t st_motor_temps;
00023       int32_t * motor_temps;
00024       float goal_pos;
00025       float current_pos;
00026       float error;
00027       float velocity;
00028       float load;
00029       bool is_moving;
00030 
00031     virtual int serialize(unsigned char *outbuffer) const
00032     {
00033       int offset = 0;
00034       offset += this->header.serialize(outbuffer + offset);
00035       uint32_t length_name = strlen(this->name);
00036       memcpy(outbuffer + offset, &length_name, sizeof(uint32_t));
00037       offset += 4;
00038       memcpy(outbuffer + offset, this->name, length_name);
00039       offset += length_name;
00040       *(outbuffer + offset++) = motor_ids_length;
00041       *(outbuffer + offset++) = 0;
00042       *(outbuffer + offset++) = 0;
00043       *(outbuffer + offset++) = 0;
00044       for( uint8_t i = 0; i < motor_ids_length; i++){
00045       union {
00046         int32_t real;
00047         uint32_t base;
00048       } u_motor_idsi;
00049       u_motor_idsi.real = this->motor_ids[i];
00050       *(outbuffer + offset + 0) = (u_motor_idsi.base >> (8 * 0)) & 0xFF;
00051       *(outbuffer + offset + 1) = (u_motor_idsi.base >> (8 * 1)) & 0xFF;
00052       *(outbuffer + offset + 2) = (u_motor_idsi.base >> (8 * 2)) & 0xFF;
00053       *(outbuffer + offset + 3) = (u_motor_idsi.base >> (8 * 3)) & 0xFF;
00054       offset += sizeof(this->motor_ids[i]);
00055       }
00056       *(outbuffer + offset++) = motor_temps_length;
00057       *(outbuffer + offset++) = 0;
00058       *(outbuffer + offset++) = 0;
00059       *(outbuffer + offset++) = 0;
00060       for( uint8_t i = 0; i < motor_temps_length; i++){
00061       union {
00062         int32_t real;
00063         uint32_t base;
00064       } u_motor_tempsi;
00065       u_motor_tempsi.real = this->motor_temps[i];
00066       *(outbuffer + offset + 0) = (u_motor_tempsi.base >> (8 * 0)) & 0xFF;
00067       *(outbuffer + offset + 1) = (u_motor_tempsi.base >> (8 * 1)) & 0xFF;
00068       *(outbuffer + offset + 2) = (u_motor_tempsi.base >> (8 * 2)) & 0xFF;
00069       *(outbuffer + offset + 3) = (u_motor_tempsi.base >> (8 * 3)) & 0xFF;
00070       offset += sizeof(this->motor_temps[i]);
00071       }
00072       int32_t * val_goal_pos = (int32_t *) &(this->goal_pos);
00073       int32_t exp_goal_pos = (((*val_goal_pos)>>23)&255);
00074       if(exp_goal_pos != 0)
00075         exp_goal_pos += 1023-127;
00076       int32_t sig_goal_pos = *val_goal_pos;
00077       *(outbuffer + offset++) = 0;
00078       *(outbuffer + offset++) = 0;
00079       *(outbuffer + offset++) = 0;
00080       *(outbuffer + offset++) = (sig_goal_pos<<5) & 0xff;
00081       *(outbuffer + offset++) = (sig_goal_pos>>3) & 0xff;
00082       *(outbuffer + offset++) = (sig_goal_pos>>11) & 0xff;
00083       *(outbuffer + offset++) = ((exp_goal_pos<<4) & 0xF0) | ((sig_goal_pos>>19)&0x0F);
00084       *(outbuffer + offset++) = (exp_goal_pos>>4) & 0x7F;
00085       if(this->goal_pos < 0) *(outbuffer + offset -1) |= 0x80;
00086       int32_t * val_current_pos = (int32_t *) &(this->current_pos);
00087       int32_t exp_current_pos = (((*val_current_pos)>>23)&255);
00088       if(exp_current_pos != 0)
00089         exp_current_pos += 1023-127;
00090       int32_t sig_current_pos = *val_current_pos;
00091       *(outbuffer + offset++) = 0;
00092       *(outbuffer + offset++) = 0;
00093       *(outbuffer + offset++) = 0;
00094       *(outbuffer + offset++) = (sig_current_pos<<5) & 0xff;
00095       *(outbuffer + offset++) = (sig_current_pos>>3) & 0xff;
00096       *(outbuffer + offset++) = (sig_current_pos>>11) & 0xff;
00097       *(outbuffer + offset++) = ((exp_current_pos<<4) & 0xF0) | ((sig_current_pos>>19)&0x0F);
00098       *(outbuffer + offset++) = (exp_current_pos>>4) & 0x7F;
00099       if(this->current_pos < 0) *(outbuffer + offset -1) |= 0x80;
00100       int32_t * val_error = (int32_t *) &(this->error);
00101       int32_t exp_error = (((*val_error)>>23)&255);
00102       if(exp_error != 0)
00103         exp_error += 1023-127;
00104       int32_t sig_error = *val_error;
00105       *(outbuffer + offset++) = 0;
00106       *(outbuffer + offset++) = 0;
00107       *(outbuffer + offset++) = 0;
00108       *(outbuffer + offset++) = (sig_error<<5) & 0xff;
00109       *(outbuffer + offset++) = (sig_error>>3) & 0xff;
00110       *(outbuffer + offset++) = (sig_error>>11) & 0xff;
00111       *(outbuffer + offset++) = ((exp_error<<4) & 0xF0) | ((sig_error>>19)&0x0F);
00112       *(outbuffer + offset++) = (exp_error>>4) & 0x7F;
00113       if(this->error < 0) *(outbuffer + offset -1) |= 0x80;
00114       int32_t * val_velocity = (int32_t *) &(this->velocity);
00115       int32_t exp_velocity = (((*val_velocity)>>23)&255);
00116       if(exp_velocity != 0)
00117         exp_velocity += 1023-127;
00118       int32_t sig_velocity = *val_velocity;
00119       *(outbuffer + offset++) = 0;
00120       *(outbuffer + offset++) = 0;
00121       *(outbuffer + offset++) = 0;
00122       *(outbuffer + offset++) = (sig_velocity<<5) & 0xff;
00123       *(outbuffer + offset++) = (sig_velocity>>3) & 0xff;
00124       *(outbuffer + offset++) = (sig_velocity>>11) & 0xff;
00125       *(outbuffer + offset++) = ((exp_velocity<<4) & 0xF0) | ((sig_velocity>>19)&0x0F);
00126       *(outbuffer + offset++) = (exp_velocity>>4) & 0x7F;
00127       if(this->velocity < 0) *(outbuffer + offset -1) |= 0x80;
00128       int32_t * val_load = (int32_t *) &(this->load);
00129       int32_t exp_load = (((*val_load)>>23)&255);
00130       if(exp_load != 0)
00131         exp_load += 1023-127;
00132       int32_t sig_load = *val_load;
00133       *(outbuffer + offset++) = 0;
00134       *(outbuffer + offset++) = 0;
00135       *(outbuffer + offset++) = 0;
00136       *(outbuffer + offset++) = (sig_load<<5) & 0xff;
00137       *(outbuffer + offset++) = (sig_load>>3) & 0xff;
00138       *(outbuffer + offset++) = (sig_load>>11) & 0xff;
00139       *(outbuffer + offset++) = ((exp_load<<4) & 0xF0) | ((sig_load>>19)&0x0F);
00140       *(outbuffer + offset++) = (exp_load>>4) & 0x7F;
00141       if(this->load < 0) *(outbuffer + offset -1) |= 0x80;
00142       union {
00143         bool real;
00144         uint8_t base;
00145       } u_is_moving;
00146       u_is_moving.real = this->is_moving;
00147       *(outbuffer + offset + 0) = (u_is_moving.base >> (8 * 0)) & 0xFF;
00148       offset += sizeof(this->is_moving);
00149       return offset;
00150     }
00151 
00152     virtual int deserialize(unsigned char *inbuffer)
00153     {
00154       int offset = 0;
00155       offset += this->header.deserialize(inbuffer + offset);
00156       uint32_t length_name;
00157       memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t));
00158       offset += 4;
00159       for(unsigned int k= offset; k< offset+length_name; ++k){
00160           inbuffer[k-1]=inbuffer[k];
00161       }
00162       inbuffer[offset+length_name-1]=0;
00163       this->name = (char *)(inbuffer + offset-1);
00164       offset += length_name;
00165       uint8_t motor_ids_lengthT = *(inbuffer + offset++);
00166       if(motor_ids_lengthT > motor_ids_length)
00167         this->motor_ids = (int32_t*)realloc(this->motor_ids, motor_ids_lengthT * sizeof(int32_t));
00168       offset += 3;
00169       motor_ids_length = motor_ids_lengthT;
00170       for( uint8_t i = 0; i < motor_ids_length; i++){
00171       union {
00172         int32_t real;
00173         uint32_t base;
00174       } u_st_motor_ids;
00175       u_st_motor_ids.base = 0;
00176       u_st_motor_ids.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00177       u_st_motor_ids.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00178       u_st_motor_ids.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00179       u_st_motor_ids.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00180       this->st_motor_ids = u_st_motor_ids.real;
00181       offset += sizeof(this->st_motor_ids);
00182         memcpy( &(this->motor_ids[i]), &(this->st_motor_ids), sizeof(int32_t));
00183       }
00184       uint8_t motor_temps_lengthT = *(inbuffer + offset++);
00185       if(motor_temps_lengthT > motor_temps_length)
00186         this->motor_temps = (int32_t*)realloc(this->motor_temps, motor_temps_lengthT * sizeof(int32_t));
00187       offset += 3;
00188       motor_temps_length = motor_temps_lengthT;
00189       for( uint8_t i = 0; i < motor_temps_length; i++){
00190       union {
00191         int32_t real;
00192         uint32_t base;
00193       } u_st_motor_temps;
00194       u_st_motor_temps.base = 0;
00195       u_st_motor_temps.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00196       u_st_motor_temps.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00197       u_st_motor_temps.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00198       u_st_motor_temps.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00199       this->st_motor_temps = u_st_motor_temps.real;
00200       offset += sizeof(this->st_motor_temps);
00201         memcpy( &(this->motor_temps[i]), &(this->st_motor_temps), sizeof(int32_t));
00202       }
00203       uint32_t * val_goal_pos = (uint32_t*) &(this->goal_pos);
00204       offset += 3;
00205       *val_goal_pos = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00206       *val_goal_pos |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00207       *val_goal_pos |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00208       *val_goal_pos |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00209       uint32_t exp_goal_pos = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00210       exp_goal_pos |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00211       if(exp_goal_pos !=0)
00212         *val_goal_pos |= ((exp_goal_pos)-1023+127)<<23;
00213       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->goal_pos = -this->goal_pos;
00214       uint32_t * val_current_pos = (uint32_t*) &(this->current_pos);
00215       offset += 3;
00216       *val_current_pos = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00217       *val_current_pos |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00218       *val_current_pos |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00219       *val_current_pos |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00220       uint32_t exp_current_pos = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00221       exp_current_pos |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00222       if(exp_current_pos !=0)
00223         *val_current_pos |= ((exp_current_pos)-1023+127)<<23;
00224       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->current_pos = -this->current_pos;
00225       uint32_t * val_error = (uint32_t*) &(this->error);
00226       offset += 3;
00227       *val_error = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00228       *val_error |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00229       *val_error |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00230       *val_error |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00231       uint32_t exp_error = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00232       exp_error |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00233       if(exp_error !=0)
00234         *val_error |= ((exp_error)-1023+127)<<23;
00235       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->error = -this->error;
00236       uint32_t * val_velocity = (uint32_t*) &(this->velocity);
00237       offset += 3;
00238       *val_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00239       *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00240       *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00241       *val_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00242       uint32_t exp_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00243       exp_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00244       if(exp_velocity !=0)
00245         *val_velocity |= ((exp_velocity)-1023+127)<<23;
00246       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->velocity = -this->velocity;
00247       uint32_t * val_load = (uint32_t*) &(this->load);
00248       offset += 3;
00249       *val_load = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00250       *val_load |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00251       *val_load |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00252       *val_load |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00253       uint32_t exp_load = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00254       exp_load |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00255       if(exp_load !=0)
00256         *val_load |= ((exp_load)-1023+127)<<23;
00257       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->load = -this->load;
00258       union {
00259         bool real;
00260         uint8_t base;
00261       } u_is_moving;
00262       u_is_moving.base = 0;
00263       u_is_moving.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00264       this->is_moving = u_is_moving.real;
00265       offset += sizeof(this->is_moving);
00266      return offset;
00267     }
00268 
00269     const char * getType(){ return "dynamixel_msgs/JointState"; };
00270     const char * getMD5(){ return "2b8449320cde76616338e2539db27c32"; };
00271 
00272   };
00273 
00274 }
00275 #endif


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:55