Go to the documentation of this file.00001 #ifndef ros_geometry_msgs_Quaternion_h
00002 #define ros_geometry_msgs_Quaternion_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "../ros/msg.h"
00008
00009 namespace geometry_msgs
00010 {
00011
00012 class Quaternion : public ros::Msg
00013 {
00014 public:
00015 float x;
00016 float y;
00017 float z;
00018 float w;
00019
00020 virtual int serialize(unsigned char *outbuffer)
00021 {
00022 int offset = 0;
00023 long * val_x = (long *) &(this->x);
00024 long exp_x = (((*val_x)>>23)&255);
00025 if(exp_x != 0)
00026 exp_x += 1023-127;
00027 long sig_x = *val_x;
00028 *(outbuffer + offset++) = 0;
00029 *(outbuffer + offset++) = 0;
00030 *(outbuffer + offset++) = 0;
00031 *(outbuffer + offset++) = (sig_x<<5) & 0xff;
00032 *(outbuffer + offset++) = (sig_x>>3) & 0xff;
00033 *(outbuffer + offset++) = (sig_x>>11) & 0xff;
00034 *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
00035 *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
00036 if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
00037 long * val_y = (long *) &(this->y);
00038 long exp_y = (((*val_y)>>23)&255);
00039 if(exp_y != 0)
00040 exp_y += 1023-127;
00041 long sig_y = *val_y;
00042 *(outbuffer + offset++) = 0;
00043 *(outbuffer + offset++) = 0;
00044 *(outbuffer + offset++) = 0;
00045 *(outbuffer + offset++) = (sig_y<<5) & 0xff;
00046 *(outbuffer + offset++) = (sig_y>>3) & 0xff;
00047 *(outbuffer + offset++) = (sig_y>>11) & 0xff;
00048 *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
00049 *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
00050 if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
00051 long * val_z = (long *) &(this->z);
00052 long exp_z = (((*val_z)>>23)&255);
00053 if(exp_z != 0)
00054 exp_z += 1023-127;
00055 long sig_z = *val_z;
00056 *(outbuffer + offset++) = 0;
00057 *(outbuffer + offset++) = 0;
00058 *(outbuffer + offset++) = 0;
00059 *(outbuffer + offset++) = (sig_z<<5) & 0xff;
00060 *(outbuffer + offset++) = (sig_z>>3) & 0xff;
00061 *(outbuffer + offset++) = (sig_z>>11) & 0xff;
00062 *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
00063 *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
00064 if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
00065 long * val_w = (long *) &(this->w);
00066 long exp_w = (((*val_w)>>23)&255);
00067 if(exp_w != 0)
00068 exp_w += 1023-127;
00069 long sig_w = *val_w;
00070 *(outbuffer + offset++) = 0;
00071 *(outbuffer + offset++) = 0;
00072 *(outbuffer + offset++) = 0;
00073 *(outbuffer + offset++) = (sig_w<<5) & 0xff;
00074 *(outbuffer + offset++) = (sig_w>>3) & 0xff;
00075 *(outbuffer + offset++) = (sig_w>>11) & 0xff;
00076 *(outbuffer + offset++) = ((exp_w<<4) & 0xF0) | ((sig_w>>19)&0x0F);
00077 *(outbuffer + offset++) = (exp_w>>4) & 0x7F;
00078 if(this->w < 0) *(outbuffer + offset -1) |= 0x80;
00079 return offset;
00080 }
00081
00082 virtual int deserialize(unsigned char *inbuffer)
00083 {
00084 int offset = 0;
00085 unsigned long * val_x = (unsigned long*) &(this->x);
00086 offset += 3;
00087 *val_x = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00088 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00089 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00090 *val_x |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00091 unsigned long exp_x = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00092 exp_x |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00093 if(exp_x !=0)
00094 *val_x |= ((exp_x)-1023+127)<<23;
00095 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
00096 unsigned long * val_y = (unsigned long*) &(this->y);
00097 offset += 3;
00098 *val_y = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00099 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00100 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00101 *val_y |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00102 unsigned long exp_y = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00103 exp_y |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00104 if(exp_y !=0)
00105 *val_y |= ((exp_y)-1023+127)<<23;
00106 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
00107 unsigned long * val_z = (unsigned long*) &(this->z);
00108 offset += 3;
00109 *val_z = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00110 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00111 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00112 *val_z |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00113 unsigned long exp_z = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00114 exp_z |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00115 if(exp_z !=0)
00116 *val_z |= ((exp_z)-1023+127)<<23;
00117 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
00118 unsigned long * val_w = (unsigned long*) &(this->w);
00119 offset += 3;
00120 *val_w = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
00121 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
00122 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
00123 *val_w |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
00124 unsigned long exp_w = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
00125 exp_w |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
00126 if(exp_w !=0)
00127 *val_w |= ((exp_w)-1023+127)<<23;
00128 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->w = -this->w;
00129 return offset;
00130 }
00131
00132 const char * getType(){ return "geometry_msgs/Quaternion"; };
00133
00134 };
00135
00136 }
00137 #endif
