Go to the documentation of this file.00001 #ifndef _ROS_pr2_controllers_msgs_SingleJointPositionFeedback_h
00002 #define _ROS_pr2_controllers_msgs_SingleJointPositionFeedback_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 pr2_controllers_msgs
00011 {
00012
00013 class SingleJointPositionFeedback : public ros::Msg
00014 {
00015 public:
00016 std_msgs::Header header;
00017 float position;
00018 float velocity;
00019 float error;
00020
00021 virtual int serialize(unsigned char *outbuffer) const
00022 {
00023 int offset = 0;
00024 offset += this->header.serialize(outbuffer + offset);
00025 int32_t * val_position = (int32_t *) &(this->position);
00026 int32_t exp_position = (((*val_position)>>23)&255);
00027 if(exp_position != 0)
00028 exp_position += 1023-127;
00029 int32_t sig_position = *val_position;
00030 *(outbuffer + offset++) = 0;
00031 *(outbuffer + offset++) = 0;
00032 *(outbuffer + offset++) = 0;
00033 *(outbuffer + offset++) = (sig_position<<5) & 0xff;
00034 *(outbuffer + offset++) = (sig_position>>3) & 0xff;
00035 *(outbuffer + offset++) = (sig_position>>11) & 0xff;
00036 *(outbuffer + offset++) = ((exp_position<<4) & 0xF0) | ((sig_position>>19)&0x0F);
00037 *(outbuffer + offset++) = (exp_position>>4) & 0x7F;
00038 if(this->position < 0) *(outbuffer + offset -1) |= 0x80;
00039 int32_t * val_velocity = (int32_t *) &(this->velocity);
00040 int32_t exp_velocity = (((*val_velocity)>>23)&255);
00041 if(exp_velocity != 0)
00042 exp_velocity += 1023-127;
00043 int32_t sig_velocity = *val_velocity;
00044 *(outbuffer + offset++) = 0;
00045 *(outbuffer + offset++) = 0;
00046 *(outbuffer + offset++) = 0;
00047 *(outbuffer + offset++) = (sig_velocity<<5) & 0xff;
00048 *(outbuffer + offset++) = (sig_velocity>>3) & 0xff;
00049 *(outbuffer + offset++) = (sig_velocity>>11) & 0xff;
00050 *(outbuffer + offset++) = ((exp_velocity<<4) & 0xF0) | ((sig_velocity>>19)&0x0F);
00051 *(outbuffer + offset++) = (exp_velocity>>4) & 0x7F;
00052 if(this->velocity < 0) *(outbuffer + offset -1) |= 0x80;
00053 int32_t * val_error = (int32_t *) &(this->error);
00054 int32_t exp_error = (((*val_error)>>23)&255);
00055 if(exp_error != 0)
00056 exp_error += 1023-127;
00057 int32_t sig_error = *val_error;
00058 *(outbuffer + offset++) = 0;
00059 *(outbuffer + offset++) = 0;
00060 *(outbuffer + offset++) = 0;
00061 *(outbuffer + offset++) = (sig_error<<5) & 0xff;
00062 *(outbuffer + offset++) = (sig_error>>3) & 0xff;
00063 *(outbuffer + offset++) = (sig_error>>11) & 0xff;
00064 *(outbuffer + offset++) = ((exp_error<<4) & 0xF0) | ((sig_error>>19)&0x0F);
00065 *(outbuffer + offset++) = (exp_error>>4) & 0x7F;
00066 if(this->error < 0) *(outbuffer + offset -1) |= 0x80;
00067 return offset;
00068 }
00069
00070 virtual int deserialize(unsigned char *inbuffer)
00071 {
00072 int offset = 0;
00073 offset += this->header.deserialize(inbuffer + offset);
00074 uint32_t * val_position = (uint32_t*) &(this->position);
00075 offset += 3;
00076 *val_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00077 *val_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00078 *val_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00079 *val_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00080 uint32_t exp_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00081 exp_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00082 if(exp_position !=0)
00083 *val_position |= ((exp_position)-1023+127)<<23;
00084 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->position = -this->position;
00085 uint32_t * val_velocity = (uint32_t*) &(this->velocity);
00086 offset += 3;
00087 *val_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00088 *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00089 *val_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00090 *val_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00091 uint32_t exp_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00092 exp_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00093 if(exp_velocity !=0)
00094 *val_velocity |= ((exp_velocity)-1023+127)<<23;
00095 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->velocity = -this->velocity;
00096 uint32_t * val_error = (uint32_t*) &(this->error);
00097 offset += 3;
00098 *val_error = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00099 *val_error |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00100 *val_error |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00101 *val_error |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00102 uint32_t exp_error = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00103 exp_error |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00104 if(exp_error !=0)
00105 *val_error |= ((exp_error)-1023+127)<<23;
00106 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->error = -this->error;
00107 return offset;
00108 }
00109
00110 const char * getType(){ return "pr2_controllers_msgs/SingleJointPositionFeedback"; };
00111 const char * getMD5(){ return "8cee65610a3d08e0a1bded82f146f1fd"; };
00112
00113 };
00114
00115 }
00116 #endif