Go to the documentation of this file.00001 #ifndef _ROS_pr2_mechanism_msgs_ActuatorStatistics_h
00002 #define _ROS_pr2_mechanism_msgs_ActuatorStatistics_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "ros/time.h"
00009
00010 namespace pr2_mechanism_msgs
00011 {
00012
00013 class ActuatorStatistics : public ros::Msg
00014 {
00015 public:
00016 const char* name;
00017 int32_t device_id;
00018 ros::Time timestamp;
00019 int32_t encoder_count;
00020 float encoder_offset;
00021 float position;
00022 float encoder_velocity;
00023 float velocity;
00024 bool calibration_reading;
00025 bool calibration_rising_edge_valid;
00026 bool calibration_falling_edge_valid;
00027 float last_calibration_rising_edge;
00028 float last_calibration_falling_edge;
00029 bool is_enabled;
00030 bool halted;
00031 float last_commanded_current;
00032 float last_commanded_effort;
00033 float last_executed_current;
00034 float last_executed_effort;
00035 float last_measured_current;
00036 float last_measured_effort;
00037 float motor_voltage;
00038 int32_t num_encoder_errors;
00039
00040 virtual int serialize(unsigned char *outbuffer) const
00041 {
00042 int offset = 0;
00043 uint32_t length_name = strlen(this->name);
00044 memcpy(outbuffer + offset, &length_name, sizeof(uint32_t));
00045 offset += 4;
00046 memcpy(outbuffer + offset, this->name, length_name);
00047 offset += length_name;
00048 union {
00049 int32_t real;
00050 uint32_t base;
00051 } u_device_id;
00052 u_device_id.real = this->device_id;
00053 *(outbuffer + offset + 0) = (u_device_id.base >> (8 * 0)) & 0xFF;
00054 *(outbuffer + offset + 1) = (u_device_id.base >> (8 * 1)) & 0xFF;
00055 *(outbuffer + offset + 2) = (u_device_id.base >> (8 * 2)) & 0xFF;
00056 *(outbuffer + offset + 3) = (u_device_id.base >> (8 * 3)) & 0xFF;
00057 offset += sizeof(this->device_id);
00058 *(outbuffer + offset + 0) = (this->timestamp.sec >> (8 * 0)) & 0xFF;
00059 *(outbuffer + offset + 1) = (this->timestamp.sec >> (8 * 1)) & 0xFF;
00060 *(outbuffer + offset + 2) = (this->timestamp.sec >> (8 * 2)) & 0xFF;
00061 *(outbuffer + offset + 3) = (this->timestamp.sec >> (8 * 3)) & 0xFF;
00062 offset += sizeof(this->timestamp.sec);
00063 *(outbuffer + offset + 0) = (this->timestamp.nsec >> (8 * 0)) & 0xFF;
00064 *(outbuffer + offset + 1) = (this->timestamp.nsec >> (8 * 1)) & 0xFF;
00065 *(outbuffer + offset + 2) = (this->timestamp.nsec >> (8 * 2)) & 0xFF;
00066 *(outbuffer + offset + 3) = (this->timestamp.nsec >> (8 * 3)) & 0xFF;
00067 offset += sizeof(this->timestamp.nsec);
00068 union {
00069 int32_t real;
00070 uint32_t base;
00071 } u_encoder_count;
00072 u_encoder_count.real = this->encoder_count;
00073 *(outbuffer + offset + 0) = (u_encoder_count.base >> (8 * 0)) & 0xFF;
00074 *(outbuffer + offset + 1) = (u_encoder_count.base >> (8 * 1)) & 0xFF;
00075 *(outbuffer + offset + 2) = (u_encoder_count.base >> (8 * 2)) & 0xFF;
00076 *(outbuffer + offset + 3) = (u_encoder_count.base >> (8 * 3)) & 0xFF;
00077 offset += sizeof(this->encoder_count);
00078 int32_t * val_encoder_offset = (int32_t *) &(this->encoder_offset);
00079 int32_t exp_encoder_offset = (((*val_encoder_offset)>>23)&255);
00080 if(exp_encoder_offset != 0)
00081 exp_encoder_offset += 1023-127;
00082 int32_t sig_encoder_offset = *val_encoder_offset;
00083 *(outbuffer + offset++) = 0;
00084 *(outbuffer + offset++) = 0;
00085 *(outbuffer + offset++) = 0;
00086 *(outbuffer + offset++) = (sig_encoder_offset<<5) & 0xff;
00087 *(outbuffer + offset++) = (sig_encoder_offset>>3) & 0xff;
00088 *(outbuffer + offset++) = (sig_encoder_offset>>11) & 0xff;
00089 *(outbuffer + offset++) = ((exp_encoder_offset<<4) & 0xF0) | ((sig_encoder_offset>>19)&0x0F);
00090 *(outbuffer + offset++) = (exp_encoder_offset>>4) & 0x7F;
00091 if(this->encoder_offset < 0) *(outbuffer + offset -1) |= 0x80;
00092 int32_t * val_position = (int32_t *) &(this->position);
00093 int32_t exp_position = (((*val_position)>>23)&255);
00094 if(exp_position != 0)
00095 exp_position += 1023-127;
00096 int32_t sig_position = *val_position;
00097 *(outbuffer + offset++) = 0;
00098 *(outbuffer + offset++) = 0;
00099 *(outbuffer + offset++) = 0;
00100 *(outbuffer + offset++) = (sig_position<<5) & 0xff;
00101 *(outbuffer + offset++) = (sig_position>>3) & 0xff;
00102 *(outbuffer + offset++) = (sig_position>>11) & 0xff;
00103 *(outbuffer + offset++) = ((exp_position<<4) & 0xF0) | ((sig_position>>19)&0x0F);
00104 *(outbuffer + offset++) = (exp_position>>4) & 0x7F;
00105 if(this->position < 0) *(outbuffer + offset -1) |= 0x80;
00106 int32_t * val_encoder_velocity = (int32_t *) &(this->encoder_velocity);
00107 int32_t exp_encoder_velocity = (((*val_encoder_velocity)>>23)&255);
00108 if(exp_encoder_velocity != 0)
00109 exp_encoder_velocity += 1023-127;
00110 int32_t sig_encoder_velocity = *val_encoder_velocity;
00111 *(outbuffer + offset++) = 0;
00112 *(outbuffer + offset++) = 0;
00113 *(outbuffer + offset++) = 0;
00114 *(outbuffer + offset++) = (sig_encoder_velocity<<5) & 0xff;
00115 *(outbuffer + offset++) = (sig_encoder_velocity>>3) & 0xff;
00116 *(outbuffer + offset++) = (sig_encoder_velocity>>11) & 0xff;
00117 *(outbuffer + offset++) = ((exp_encoder_velocity<<4) & 0xF0) | ((sig_encoder_velocity>>19)&0x0F);
00118 *(outbuffer + offset++) = (exp_encoder_velocity>>4) & 0x7F;
00119 if(this->encoder_velocity < 0) *(outbuffer + offset -1) |= 0x80;
00120 int32_t * val_velocity = (int32_t *) &(this->velocity);
00121 int32_t exp_velocity = (((*val_velocity)>>23)&255);
00122 if(exp_velocity != 0)
00123 exp_velocity += 1023-127;
00124 int32_t sig_velocity = *val_velocity;
00125 *(outbuffer + offset++) = 0;
00126 *(outbuffer + offset++) = 0;
00127 *(outbuffer + offset++) = 0;
00128 *(outbuffer + offset++) = (sig_velocity<<5) & 0xff;
00129 *(outbuffer + offset++) = (sig_velocity>>3) & 0xff;
00130 *(outbuffer + offset++) = (sig_velocity>>11) & 0xff;
00131 *(outbuffer + offset++) = ((exp_velocity<<4) & 0xF0) | ((sig_velocity>>19)&0x0F);
00132 *(outbuffer + offset++) = (exp_velocity>>4) & 0x7F;
00133 if(this->velocity < 0) *(outbuffer + offset -1) |= 0x80;
00134 union {
00135 bool real;
00136 uint8_t base;
00137 } u_calibration_reading;
00138 u_calibration_reading.real = this->calibration_reading;
00139 *(outbuffer + offset + 0) = (u_calibration_reading.base >> (8 * 0)) & 0xFF;
00140 offset += sizeof(this->calibration_reading);
00141 union {
00142 bool real;
00143 uint8_t base;
00144 } u_calibration_rising_edge_valid;
00145 u_calibration_rising_edge_valid.real = this->calibration_rising_edge_valid;
00146 *(outbuffer + offset + 0) = (u_calibration_rising_edge_valid.base >> (8 * 0)) & 0xFF;
00147 offset += sizeof(this->calibration_rising_edge_valid);
00148 union {
00149 bool real;
00150 uint8_t base;
00151 } u_calibration_falling_edge_valid;
00152 u_calibration_falling_edge_valid.real = this->calibration_falling_edge_valid;
00153 *(outbuffer + offset + 0) = (u_calibration_falling_edge_valid.base >> (8 * 0)) & 0xFF;
00154 offset += sizeof(this->calibration_falling_edge_valid);
00155 int32_t * val_last_calibration_rising_edge = (int32_t *) &(this->last_calibration_rising_edge);
00156 int32_t exp_last_calibration_rising_edge = (((*val_last_calibration_rising_edge)>>23)&255);
00157 if(exp_last_calibration_rising_edge != 0)
00158 exp_last_calibration_rising_edge += 1023-127;
00159 int32_t sig_last_calibration_rising_edge = *val_last_calibration_rising_edge;
00160 *(outbuffer + offset++) = 0;
00161 *(outbuffer + offset++) = 0;
00162 *(outbuffer + offset++) = 0;
00163 *(outbuffer + offset++) = (sig_last_calibration_rising_edge<<5) & 0xff;
00164 *(outbuffer + offset++) = (sig_last_calibration_rising_edge>>3) & 0xff;
00165 *(outbuffer + offset++) = (sig_last_calibration_rising_edge>>11) & 0xff;
00166 *(outbuffer + offset++) = ((exp_last_calibration_rising_edge<<4) & 0xF0) | ((sig_last_calibration_rising_edge>>19)&0x0F);
00167 *(outbuffer + offset++) = (exp_last_calibration_rising_edge>>4) & 0x7F;
00168 if(this->last_calibration_rising_edge < 0) *(outbuffer + offset -1) |= 0x80;
00169 int32_t * val_last_calibration_falling_edge = (int32_t *) &(this->last_calibration_falling_edge);
00170 int32_t exp_last_calibration_falling_edge = (((*val_last_calibration_falling_edge)>>23)&255);
00171 if(exp_last_calibration_falling_edge != 0)
00172 exp_last_calibration_falling_edge += 1023-127;
00173 int32_t sig_last_calibration_falling_edge = *val_last_calibration_falling_edge;
00174 *(outbuffer + offset++) = 0;
00175 *(outbuffer + offset++) = 0;
00176 *(outbuffer + offset++) = 0;
00177 *(outbuffer + offset++) = (sig_last_calibration_falling_edge<<5) & 0xff;
00178 *(outbuffer + offset++) = (sig_last_calibration_falling_edge>>3) & 0xff;
00179 *(outbuffer + offset++) = (sig_last_calibration_falling_edge>>11) & 0xff;
00180 *(outbuffer + offset++) = ((exp_last_calibration_falling_edge<<4) & 0xF0) | ((sig_last_calibration_falling_edge>>19)&0x0F);
00181 *(outbuffer + offset++) = (exp_last_calibration_falling_edge>>4) & 0x7F;
00182 if(this->last_calibration_falling_edge < 0) *(outbuffer + offset -1) |= 0x80;
00183 union {
00184 bool real;
00185 uint8_t base;
00186 } u_is_enabled;
00187 u_is_enabled.real = this->is_enabled;
00188 *(outbuffer + offset + 0) = (u_is_enabled.base >> (8 * 0)) & 0xFF;
00189 offset += sizeof(this->is_enabled);
00190 union {
00191 bool real;
00192 uint8_t base;
00193 } u_halted;
00194 u_halted.real = this->halted;
00195 *(outbuffer + offset + 0) = (u_halted.base >> (8 * 0)) & 0xFF;
00196 offset += sizeof(this->halted);
00197 int32_t * val_last_commanded_current = (int32_t *) &(this->last_commanded_current);
00198 int32_t exp_last_commanded_current = (((*val_last_commanded_current)>>23)&255);
00199 if(exp_last_commanded_current != 0)
00200 exp_last_commanded_current += 1023-127;
00201 int32_t sig_last_commanded_current = *val_last_commanded_current;
00202 *(outbuffer + offset++) = 0;
00203 *(outbuffer + offset++) = 0;
00204 *(outbuffer + offset++) = 0;
00205 *(outbuffer + offset++) = (sig_last_commanded_current<<5) & 0xff;
00206 *(outbuffer + offset++) = (sig_last_commanded_current>>3) & 0xff;
00207 *(outbuffer + offset++) = (sig_last_commanded_current>>11) & 0xff;
00208 *(outbuffer + offset++) = ((exp_last_commanded_current<<4) & 0xF0) | ((sig_last_commanded_current>>19)&0x0F);
00209 *(outbuffer + offset++) = (exp_last_commanded_current>>4) & 0x7F;
00210 if(this->last_commanded_current < 0) *(outbuffer + offset -1) |= 0x80;
00211 int32_t * val_last_commanded_effort = (int32_t *) &(this->last_commanded_effort);
00212 int32_t exp_last_commanded_effort = (((*val_last_commanded_effort)>>23)&255);
00213 if(exp_last_commanded_effort != 0)
00214 exp_last_commanded_effort += 1023-127;
00215 int32_t sig_last_commanded_effort = *val_last_commanded_effort;
00216 *(outbuffer + offset++) = 0;
00217 *(outbuffer + offset++) = 0;
00218 *(outbuffer + offset++) = 0;
00219 *(outbuffer + offset++) = (sig_last_commanded_effort<<5) & 0xff;
00220 *(outbuffer + offset++) = (sig_last_commanded_effort>>3) & 0xff;
00221 *(outbuffer + offset++) = (sig_last_commanded_effort>>11) & 0xff;
00222 *(outbuffer + offset++) = ((exp_last_commanded_effort<<4) & 0xF0) | ((sig_last_commanded_effort>>19)&0x0F);
00223 *(outbuffer + offset++) = (exp_last_commanded_effort>>4) & 0x7F;
00224 if(this->last_commanded_effort < 0) *(outbuffer + offset -1) |= 0x80;
00225 int32_t * val_last_executed_current = (int32_t *) &(this->last_executed_current);
00226 int32_t exp_last_executed_current = (((*val_last_executed_current)>>23)&255);
00227 if(exp_last_executed_current != 0)
00228 exp_last_executed_current += 1023-127;
00229 int32_t sig_last_executed_current = *val_last_executed_current;
00230 *(outbuffer + offset++) = 0;
00231 *(outbuffer + offset++) = 0;
00232 *(outbuffer + offset++) = 0;
00233 *(outbuffer + offset++) = (sig_last_executed_current<<5) & 0xff;
00234 *(outbuffer + offset++) = (sig_last_executed_current>>3) & 0xff;
00235 *(outbuffer + offset++) = (sig_last_executed_current>>11) & 0xff;
00236 *(outbuffer + offset++) = ((exp_last_executed_current<<4) & 0xF0) | ((sig_last_executed_current>>19)&0x0F);
00237 *(outbuffer + offset++) = (exp_last_executed_current>>4) & 0x7F;
00238 if(this->last_executed_current < 0) *(outbuffer + offset -1) |= 0x80;
00239 int32_t * val_last_executed_effort = (int32_t *) &(this->last_executed_effort);
00240 int32_t exp_last_executed_effort = (((*val_last_executed_effort)>>23)&255);
00241 if(exp_last_executed_effort != 0)
00242 exp_last_executed_effort += 1023-127;
00243 int32_t sig_last_executed_effort = *val_last_executed_effort;
00244 *(outbuffer + offset++) = 0;
00245 *(outbuffer + offset++) = 0;
00246 *(outbuffer + offset++) = 0;
00247 *(outbuffer + offset++) = (sig_last_executed_effort<<5) & 0xff;
00248 *(outbuffer + offset++) = (sig_last_executed_effort>>3) & 0xff;
00249 *(outbuffer + offset++) = (sig_last_executed_effort>>11) & 0xff;
00250 *(outbuffer + offset++) = ((exp_last_executed_effort<<4) & 0xF0) | ((sig_last_executed_effort>>19)&0x0F);
00251 *(outbuffer + offset++) = (exp_last_executed_effort>>4) & 0x7F;
00252 if(this->last_executed_effort < 0) *(outbuffer + offset -1) |= 0x80;
00253 int32_t * val_last_measured_current = (int32_t *) &(this->last_measured_current);
00254 int32_t exp_last_measured_current = (((*val_last_measured_current)>>23)&255);
00255 if(exp_last_measured_current != 0)
00256 exp_last_measured_current += 1023-127;
00257 int32_t sig_last_measured_current = *val_last_measured_current;
00258 *(outbuffer + offset++) = 0;
00259 *(outbuffer + offset++) = 0;
00260 *(outbuffer + offset++) = 0;
00261 *(outbuffer + offset++) = (sig_last_measured_current<<5) & 0xff;
00262 *(outbuffer + offset++) = (sig_last_measured_current>>3) & 0xff;
00263 *(outbuffer + offset++) = (sig_last_measured_current>>11) & 0xff;
00264 *(outbuffer + offset++) = ((exp_last_measured_current<<4) & 0xF0) | ((sig_last_measured_current>>19)&0x0F);
00265 *(outbuffer + offset++) = (exp_last_measured_current>>4) & 0x7F;
00266 if(this->last_measured_current < 0) *(outbuffer + offset -1) |= 0x80;
00267 int32_t * val_last_measured_effort = (int32_t *) &(this->last_measured_effort);
00268 int32_t exp_last_measured_effort = (((*val_last_measured_effort)>>23)&255);
00269 if(exp_last_measured_effort != 0)
00270 exp_last_measured_effort += 1023-127;
00271 int32_t sig_last_measured_effort = *val_last_measured_effort;
00272 *(outbuffer + offset++) = 0;
00273 *(outbuffer + offset++) = 0;
00274 *(outbuffer + offset++) = 0;
00275 *(outbuffer + offset++) = (sig_last_measured_effort<<5) & 0xff;
00276 *(outbuffer + offset++) = (sig_last_measured_effort>>3) & 0xff;
00277 *(outbuffer + offset++) = (sig_last_measured_effort>>11) & 0xff;
00278 *(outbuffer + offset++) = ((exp_last_measured_effort<<4) & 0xF0) | ((sig_last_measured_effort>>19)&0x0F);
00279 *(outbuffer + offset++) = (exp_last_measured_effort>>4) & 0x7F;
00280 if(this->last_measured_effort < 0) *(outbuffer + offset -1) |= 0x80;
00281 int32_t * val_motor_voltage = (int32_t *) &(this->motor_voltage);
00282 int32_t exp_motor_voltage = (((*val_motor_voltage)>>23)&255);
00283 if(exp_motor_voltage != 0)
00284 exp_motor_voltage += 1023-127;
00285 int32_t sig_motor_voltage = *val_motor_voltage;
00286 *(outbuffer + offset++) = 0;
00287 *(outbuffer + offset++) = 0;
00288 *(outbuffer + offset++) = 0;
00289 *(outbuffer + offset++) = (sig_motor_voltage<<5) & 0xff;
00290 *(outbuffer + offset++) = (sig_motor_voltage>>3) & 0xff;
00291 *(outbuffer + offset++) = (sig_motor_voltage>>11) & 0xff;
00292 *(outbuffer + offset++) = ((exp_motor_voltage<<4) & 0xF0) | ((sig_motor_voltage>>19)&0x0F);
00293 *(outbuffer + offset++) = (exp_motor_voltage>>4) & 0x7F;
00294 if(this->motor_voltage < 0) *(outbuffer + offset -1) |= 0x80;
00295 union {
00296 int32_t real;
00297 uint32_t base;
00298 } u_num_encoder_errors;
00299 u_num_encoder_errors.real = this->num_encoder_errors;
00300 *(outbuffer + offset + 0) = (u_num_encoder_errors.base >> (8 * 0)) & 0xFF;
00301 *(outbuffer + offset + 1) = (u_num_encoder_errors.base >> (8 * 1)) & 0xFF;
00302 *(outbuffer + offset + 2) = (u_num_encoder_errors.base >> (8 * 2)) & 0xFF;
00303 *(outbuffer + offset + 3) = (u_num_encoder_errors.base >> (8 * 3)) & 0xFF;
00304 offset += sizeof(this->num_encoder_errors);
00305 return offset;
00306 }
00307
00308 virtual int deserialize(unsigned char *inbuffer)
00309 {
00310 int offset = 0;
00311 uint32_t length_name;
00312 memcpy(&length_name, (inbuffer + offset), sizeof(uint32_t));
00313 offset += 4;
00314 for(unsigned int k= offset; k< offset+length_name; ++k){
00315 inbuffer[k-1]=inbuffer[k];
00316 }
00317 inbuffer[offset+length_name-1]=0;
00318 this->name = (char *)(inbuffer + offset-1);
00319 offset += length_name;
00320 union {
00321 int32_t real;
00322 uint32_t base;
00323 } u_device_id;
00324 u_device_id.base = 0;
00325 u_device_id.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00326 u_device_id.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00327 u_device_id.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00328 u_device_id.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00329 this->device_id = u_device_id.real;
00330 offset += sizeof(this->device_id);
00331 this->timestamp.sec = ((uint32_t) (*(inbuffer + offset)));
00332 this->timestamp.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00333 this->timestamp.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00334 this->timestamp.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00335 offset += sizeof(this->timestamp.sec);
00336 this->timestamp.nsec = ((uint32_t) (*(inbuffer + offset)));
00337 this->timestamp.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00338 this->timestamp.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00339 this->timestamp.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00340 offset += sizeof(this->timestamp.nsec);
00341 union {
00342 int32_t real;
00343 uint32_t base;
00344 } u_encoder_count;
00345 u_encoder_count.base = 0;
00346 u_encoder_count.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00347 u_encoder_count.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00348 u_encoder_count.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00349 u_encoder_count.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00350 this->encoder_count = u_encoder_count.real;
00351 offset += sizeof(this->encoder_count);
00352 uint32_t * val_encoder_offset = (uint32_t*) &(this->encoder_offset);
00353 offset += 3;
00354 *val_encoder_offset = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00355 *val_encoder_offset |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00356 *val_encoder_offset |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00357 *val_encoder_offset |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00358 uint32_t exp_encoder_offset = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00359 exp_encoder_offset |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00360 if(exp_encoder_offset !=0)
00361 *val_encoder_offset |= ((exp_encoder_offset)-1023+127)<<23;
00362 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->encoder_offset = -this->encoder_offset;
00363 uint32_t * val_position = (uint32_t*) &(this->position);
00364 offset += 3;
00365 *val_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00366 *val_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00367 *val_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00368 *val_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00369 uint32_t exp_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00370 exp_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00371 if(exp_position !=0)
00372 *val_position |= ((exp_position)-1023+127)<<23;
00373 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->position = -this->position;
00374 uint32_t * val_encoder_velocity = (uint32_t*) &(this->encoder_velocity);
00375 offset += 3;
00376 *val_encoder_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00377 *val_encoder_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00378 *val_encoder_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00379 *val_encoder_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00380 uint32_t exp_encoder_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00381 exp_encoder_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00382 if(exp_encoder_velocity !=0)
00383 *val_encoder_velocity |= ((exp_encoder_velocity)-1023+127)<<23;
00384 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->encoder_velocity = -this->encoder_velocity;
00385 uint32_t * val_velocity = (uint32_t*) &(this->velocity);
00386 offset += 3;
00387 *val_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00388 *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00389 *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00390 *val_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00391 uint32_t exp_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00392 exp_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00393 if(exp_velocity !=0)
00394 *val_velocity |= ((exp_velocity)-1023+127)<<23;
00395 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->velocity = -this->velocity;
00396 union {
00397 bool real;
00398 uint8_t base;
00399 } u_calibration_reading;
00400 u_calibration_reading.base = 0;
00401 u_calibration_reading.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00402 this->calibration_reading = u_calibration_reading.real;
00403 offset += sizeof(this->calibration_reading);
00404 union {
00405 bool real;
00406 uint8_t base;
00407 } u_calibration_rising_edge_valid;
00408 u_calibration_rising_edge_valid.base = 0;
00409 u_calibration_rising_edge_valid.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00410 this->calibration_rising_edge_valid = u_calibration_rising_edge_valid.real;
00411 offset += sizeof(this->calibration_rising_edge_valid);
00412 union {
00413 bool real;
00414 uint8_t base;
00415 } u_calibration_falling_edge_valid;
00416 u_calibration_falling_edge_valid.base = 0;
00417 u_calibration_falling_edge_valid.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00418 this->calibration_falling_edge_valid = u_calibration_falling_edge_valid.real;
00419 offset += sizeof(this->calibration_falling_edge_valid);
00420 uint32_t * val_last_calibration_rising_edge = (uint32_t*) &(this->last_calibration_rising_edge);
00421 offset += 3;
00422 *val_last_calibration_rising_edge = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00423 *val_last_calibration_rising_edge |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00424 *val_last_calibration_rising_edge |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00425 *val_last_calibration_rising_edge |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00426 uint32_t exp_last_calibration_rising_edge = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00427 exp_last_calibration_rising_edge |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00428 if(exp_last_calibration_rising_edge !=0)
00429 *val_last_calibration_rising_edge |= ((exp_last_calibration_rising_edge)-1023+127)<<23;
00430 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_calibration_rising_edge = -this->last_calibration_rising_edge;
00431 uint32_t * val_last_calibration_falling_edge = (uint32_t*) &(this->last_calibration_falling_edge);
00432 offset += 3;
00433 *val_last_calibration_falling_edge = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00434 *val_last_calibration_falling_edge |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00435 *val_last_calibration_falling_edge |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00436 *val_last_calibration_falling_edge |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00437 uint32_t exp_last_calibration_falling_edge = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00438 exp_last_calibration_falling_edge |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00439 if(exp_last_calibration_falling_edge !=0)
00440 *val_last_calibration_falling_edge |= ((exp_last_calibration_falling_edge)-1023+127)<<23;
00441 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_calibration_falling_edge = -this->last_calibration_falling_edge;
00442 union {
00443 bool real;
00444 uint8_t base;
00445 } u_is_enabled;
00446 u_is_enabled.base = 0;
00447 u_is_enabled.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00448 this->is_enabled = u_is_enabled.real;
00449 offset += sizeof(this->is_enabled);
00450 union {
00451 bool real;
00452 uint8_t base;
00453 } u_halted;
00454 u_halted.base = 0;
00455 u_halted.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00456 this->halted = u_halted.real;
00457 offset += sizeof(this->halted);
00458 uint32_t * val_last_commanded_current = (uint32_t*) &(this->last_commanded_current);
00459 offset += 3;
00460 *val_last_commanded_current = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00461 *val_last_commanded_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00462 *val_last_commanded_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00463 *val_last_commanded_current |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00464 uint32_t exp_last_commanded_current = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00465 exp_last_commanded_current |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00466 if(exp_last_commanded_current !=0)
00467 *val_last_commanded_current |= ((exp_last_commanded_current)-1023+127)<<23;
00468 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_commanded_current = -this->last_commanded_current;
00469 uint32_t * val_last_commanded_effort = (uint32_t*) &(this->last_commanded_effort);
00470 offset += 3;
00471 *val_last_commanded_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00472 *val_last_commanded_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00473 *val_last_commanded_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00474 *val_last_commanded_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00475 uint32_t exp_last_commanded_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00476 exp_last_commanded_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00477 if(exp_last_commanded_effort !=0)
00478 *val_last_commanded_effort |= ((exp_last_commanded_effort)-1023+127)<<23;
00479 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_commanded_effort = -this->last_commanded_effort;
00480 uint32_t * val_last_executed_current = (uint32_t*) &(this->last_executed_current);
00481 offset += 3;
00482 *val_last_executed_current = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00483 *val_last_executed_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00484 *val_last_executed_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00485 *val_last_executed_current |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00486 uint32_t exp_last_executed_current = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00487 exp_last_executed_current |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00488 if(exp_last_executed_current !=0)
00489 *val_last_executed_current |= ((exp_last_executed_current)-1023+127)<<23;
00490 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_executed_current = -this->last_executed_current;
00491 uint32_t * val_last_executed_effort = (uint32_t*) &(this->last_executed_effort);
00492 offset += 3;
00493 *val_last_executed_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00494 *val_last_executed_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00495 *val_last_executed_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00496 *val_last_executed_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00497 uint32_t exp_last_executed_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00498 exp_last_executed_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00499 if(exp_last_executed_effort !=0)
00500 *val_last_executed_effort |= ((exp_last_executed_effort)-1023+127)<<23;
00501 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_executed_effort = -this->last_executed_effort;
00502 uint32_t * val_last_measured_current = (uint32_t*) &(this->last_measured_current);
00503 offset += 3;
00504 *val_last_measured_current = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00505 *val_last_measured_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00506 *val_last_measured_current |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00507 *val_last_measured_current |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00508 uint32_t exp_last_measured_current = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00509 exp_last_measured_current |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00510 if(exp_last_measured_current !=0)
00511 *val_last_measured_current |= ((exp_last_measured_current)-1023+127)<<23;
00512 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_measured_current = -this->last_measured_current;
00513 uint32_t * val_last_measured_effort = (uint32_t*) &(this->last_measured_effort);
00514 offset += 3;
00515 *val_last_measured_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00516 *val_last_measured_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00517 *val_last_measured_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00518 *val_last_measured_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00519 uint32_t exp_last_measured_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00520 exp_last_measured_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00521 if(exp_last_measured_effort !=0)
00522 *val_last_measured_effort |= ((exp_last_measured_effort)-1023+127)<<23;
00523 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->last_measured_effort = -this->last_measured_effort;
00524 uint32_t * val_motor_voltage = (uint32_t*) &(this->motor_voltage);
00525 offset += 3;
00526 *val_motor_voltage = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00527 *val_motor_voltage |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00528 *val_motor_voltage |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00529 *val_motor_voltage |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00530 uint32_t exp_motor_voltage = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00531 exp_motor_voltage |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00532 if(exp_motor_voltage !=0)
00533 *val_motor_voltage |= ((exp_motor_voltage)-1023+127)<<23;
00534 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->motor_voltage = -this->motor_voltage;
00535 union {
00536 int32_t real;
00537 uint32_t base;
00538 } u_num_encoder_errors;
00539 u_num_encoder_errors.base = 0;
00540 u_num_encoder_errors.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00541 u_num_encoder_errors.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00542 u_num_encoder_errors.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00543 u_num_encoder_errors.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00544 this->num_encoder_errors = u_num_encoder_errors.real;
00545 offset += sizeof(this->num_encoder_errors);
00546 return offset;
00547 }
00548
00549 const char * getType(){ return "pr2_mechanism_msgs/ActuatorStatistics"; };
00550 const char * getMD5(){ return "c37184273b29627de29382f1d3670175"; };
00551
00552 };
00553
00554 }
00555 #endif