Gary Servin
/
rosserial_mbed_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
rosserial_mbed_lib
by 
nucho
Embed:
(wiki syntax)
Show/hide line numbers
Quaternion.h
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) const 00021 { 00022 int offset = 0; 00023 int32_t * val_x = (long *) &(this->x); 00024 int32_t exp_x = (((*val_x)>>23)&255); 00025 if(exp_x != 0) 00026 exp_x += 1023-127; 00027 int32_t 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 int32_t * val_y = (long *) &(this->y); 00038 int32_t exp_y = (((*val_y)>>23)&255); 00039 if(exp_y != 0) 00040 exp_y += 1023-127; 00041 int32_t 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 int32_t * val_z = (long *) &(this->z); 00052 int32_t exp_z = (((*val_z)>>23)&255); 00053 if(exp_z != 0) 00054 exp_z += 1023-127; 00055 int32_t 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 int32_t * val_w = (long *) &(this->w); 00066 int32_t exp_w = (((*val_w)>>23)&255); 00067 if(exp_w != 0) 00068 exp_w += 1023-127; 00069 int32_t 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 uint32_t * val_x = (uint32_t*) &(this->x); 00086 offset += 3; 00087 *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00088 *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00089 *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00090 *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00091 uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00092 exp_x |= ((uint32_t)(*(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 uint32_t * val_y = (uint32_t*) &(this->y); 00097 offset += 3; 00098 *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00099 *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00100 *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00101 *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00102 uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00103 exp_y |= ((uint32_t)(*(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 uint32_t * val_z = (uint32_t*) &(this->z); 00108 offset += 3; 00109 *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00110 *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00111 *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00112 *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00113 uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00114 exp_z |= ((uint32_t)(*(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 uint32_t * val_w = (uint32_t*) &(this->w); 00119 offset += 3; 00120 *val_w = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00121 *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00122 *val_w |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00123 *val_w |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00124 uint32_t exp_w = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00125 exp_w |= ((uint32_t)(*(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 virtual const char * getType(){ return "geometry_msgs/Quaternion"; }; 00133 virtual const char * getMD5(){ return "a779879fadf0160734f906b8c19c7004"; }; 00134 00135 }; 00136 00137 } 00138 #endif
Generated on Tue Jul 12 2022 19:53:57 by
1.7.2