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