nucho / rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:  

Dependents:   rosserial_mbed robot_S2

geometry_msgs/Quaternion.h@1:ff0ec969dad1, 2011-10-16 (annotated)

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
This program supported the revision of 143 of rosserial.

And the bug fix of receive of array data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 1:ff0ec969dad1 1 #ifndef ros_geometry_msgs_Quaternion_h
nucho 1:ff0ec969dad1 2 #define ros_geometry_msgs_Quaternion_h
nucho 0:77afd7560544 3
nucho 0:77afd7560544 4 #include <stdint.h>
nucho 0:77afd7560544 5 #include <string.h>
nucho 0:77afd7560544 6 #include <stdlib.h>
nucho 0:77afd7560544 7 #include "../ros/msg.h"
nucho 0:77afd7560544 8
nucho 0:77afd7560544 9 namespace geometry_msgs
nucho 0:77afd7560544 10 {
nucho 0:77afd7560544 11
nucho 0:77afd7560544 12 class Quaternion : public ros::Msg
nucho 0:77afd7560544 13 {
nucho 0:77afd7560544 14 public:
nucho 0:77afd7560544 15 float x;
nucho 0:77afd7560544 16 float y;
nucho 0:77afd7560544 17 float z;
nucho 0:77afd7560544 18 float w;
nucho 0:77afd7560544 19
nucho 0:77afd7560544 20 virtual int serialize(unsigned char *outbuffer)
nucho 0:77afd7560544 21 {
nucho 0:77afd7560544 22 int offset = 0;
nucho 0:77afd7560544 23 long * val_x = (long *) &(this->x);
nucho 0:77afd7560544 24 long exp_x = (((*val_x)>>23)&255);
nucho 0:77afd7560544 25 if(exp_x != 0)
nucho 0:77afd7560544 26 exp_x += 1023-127;
nucho 0:77afd7560544 27 long sig_x = *val_x;
nucho 0:77afd7560544 28 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 29 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 30 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 31 *(outbuffer + offset++) = (sig_x<<5) & 0xff;
nucho 0:77afd7560544 32 *(outbuffer + offset++) = (sig_x>>3) & 0xff;
nucho 0:77afd7560544 33 *(outbuffer + offset++) = (sig_x>>11) & 0xff;
nucho 0:77afd7560544 34 *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
nucho 0:77afd7560544 35 *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
nucho 0:77afd7560544 36 if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 37 long * val_y = (long *) &(this->y);
nucho 0:77afd7560544 38 long exp_y = (((*val_y)>>23)&255);
nucho 0:77afd7560544 39 if(exp_y != 0)
nucho 0:77afd7560544 40 exp_y += 1023-127;
nucho 0:77afd7560544 41 long sig_y = *val_y;
nucho 0:77afd7560544 42 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 43 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 44 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 45 *(outbuffer + offset++) = (sig_y<<5) & 0xff;
nucho 0:77afd7560544 46 *(outbuffer + offset++) = (sig_y>>3) & 0xff;
nucho 0:77afd7560544 47 *(outbuffer + offset++) = (sig_y>>11) & 0xff;
nucho 0:77afd7560544 48 *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
nucho 0:77afd7560544 49 *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
nucho 0:77afd7560544 50 if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 51 long * val_z = (long *) &(this->z);
nucho 0:77afd7560544 52 long exp_z = (((*val_z)>>23)&255);
nucho 0:77afd7560544 53 if(exp_z != 0)
nucho 0:77afd7560544 54 exp_z += 1023-127;
nucho 0:77afd7560544 55 long sig_z = *val_z;
nucho 0:77afd7560544 56 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 57 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 58 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 59 *(outbuffer + offset++) = (sig_z<<5) & 0xff;
nucho 0:77afd7560544 60 *(outbuffer + offset++) = (sig_z>>3) & 0xff;
nucho 0:77afd7560544 61 *(outbuffer + offset++) = (sig_z>>11) & 0xff;
nucho 0:77afd7560544 62 *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
nucho 0:77afd7560544 63 *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
nucho 0:77afd7560544 64 if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 65 long * val_w = (long *) &(this->w);
nucho 0:77afd7560544 66 long exp_w = (((*val_w)>>23)&255);
nucho 0:77afd7560544 67 if(exp_w != 0)
nucho 0:77afd7560544 68 exp_w += 1023-127;
nucho 0:77afd7560544 69 long sig_w = *val_w;
nucho 0:77afd7560544 70 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 71 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 72 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 73 *(outbuffer + offset++) = (sig_w<<5) & 0xff;
nucho 0:77afd7560544 74 *(outbuffer + offset++) = (sig_w>>3) & 0xff;
nucho 0:77afd7560544 75 *(outbuffer + offset++) = (sig_w>>11) & 0xff;
nucho 0:77afd7560544 76 *(outbuffer + offset++) = ((exp_w<<4) & 0xF0) | ((sig_w>>19)&0x0F);
nucho 0:77afd7560544 77 *(outbuffer + offset++) = (exp_w>>4) & 0x7F;
nucho 0:77afd7560544 78 if(this->w < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 79 return offset;
nucho 0:77afd7560544 80 }
nucho 0:77afd7560544 81
nucho 0:77afd7560544 82 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 83 {
nucho 0:77afd7560544 84 int offset = 0;
nucho 0:77afd7560544 85 unsigned long * val_x = (unsigned long*) &(this->x);
nucho 0:77afd7560544 86 offset += 3;
nucho 0:77afd7560544 87 *val_x = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 88 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 89 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 90 *val_x |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 91 unsigned long exp_x = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 92 exp_x |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 93 if(exp_x !=0)
nucho 0:77afd7560544 94 *val_x |= ((exp_x)-1023+127)<<23;
nucho 0:77afd7560544 95 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
nucho 0:77afd7560544 96 unsigned long * val_y = (unsigned long*) &(this->y);
nucho 0:77afd7560544 97 offset += 3;
nucho 0:77afd7560544 98 *val_y = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 99 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 100 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 101 *val_y |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 102 unsigned long exp_y = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 103 exp_y |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 104 if(exp_y !=0)
nucho 0:77afd7560544 105 *val_y |= ((exp_y)-1023+127)<<23;
nucho 0:77afd7560544 106 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
nucho 0:77afd7560544 107 unsigned long * val_z = (unsigned long*) &(this->z);
nucho 0:77afd7560544 108 offset += 3;
nucho 0:77afd7560544 109 *val_z = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 110 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 111 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 112 *val_z |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 113 unsigned long exp_z = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 114 exp_z |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 115 if(exp_z !=0)
nucho 0:77afd7560544 116 *val_z |= ((exp_z)-1023+127)<<23;
nucho 0:77afd7560544 117 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
nucho 0:77afd7560544 118 unsigned long * val_w = (unsigned long*) &(this->w);
nucho 0:77afd7560544 119 offset += 3;
nucho 0:77afd7560544 120 *val_w = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 121 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 122 *val_w |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 123 *val_w |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 124 unsigned long exp_w = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 125 exp_w |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 126 if(exp_w !=0)
nucho 0:77afd7560544 127 *val_w |= ((exp_w)-1023+127)<<23;
nucho 0:77afd7560544 128 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->w = -this->w;
nucho 0:77afd7560544 129 return offset;
nucho 0:77afd7560544 130 }
nucho 0:77afd7560544 131
nucho 0:77afd7560544 132 virtual const char * getType(){ return "geometry_msgs/Quaternion"; };
nucho 0:77afd7560544 133
nucho 0:77afd7560544 134 };
nucho 0:77afd7560544 135
nucho 0:77afd7560544 136 }
nucho 0:77afd7560544 137 #endif