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/Vector3.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_Vector3_h
nucho 1:ff0ec969dad1 2 #define ros_geometry_msgs_Vector3_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 Vector3 : 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
nucho 0:77afd7560544 19 virtual int serialize(unsigned char *outbuffer)
nucho 0:77afd7560544 20 {
nucho 0:77afd7560544 21 int offset = 0;
nucho 0:77afd7560544 22 long * val_x = (long *) &(this->x);
nucho 0:77afd7560544 23 long exp_x = (((*val_x)>>23)&255);
nucho 0:77afd7560544 24 if(exp_x != 0)
nucho 0:77afd7560544 25 exp_x += 1023-127;
nucho 0:77afd7560544 26 long sig_x = *val_x;
nucho 0:77afd7560544 27 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 28 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 29 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 30 *(outbuffer + offset++) = (sig_x<<5) & 0xff;
nucho 0:77afd7560544 31 *(outbuffer + offset++) = (sig_x>>3) & 0xff;
nucho 0:77afd7560544 32 *(outbuffer + offset++) = (sig_x>>11) & 0xff;
nucho 0:77afd7560544 33 *(outbuffer + offset++) = ((exp_x<<4) & 0xF0) | ((sig_x>>19)&0x0F);
nucho 0:77afd7560544 34 *(outbuffer + offset++) = (exp_x>>4) & 0x7F;
nucho 0:77afd7560544 35 if(this->x < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 36 long * val_y = (long *) &(this->y);
nucho 0:77afd7560544 37 long exp_y = (((*val_y)>>23)&255);
nucho 0:77afd7560544 38 if(exp_y != 0)
nucho 0:77afd7560544 39 exp_y += 1023-127;
nucho 0:77afd7560544 40 long sig_y = *val_y;
nucho 0:77afd7560544 41 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 42 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 43 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 44 *(outbuffer + offset++) = (sig_y<<5) & 0xff;
nucho 0:77afd7560544 45 *(outbuffer + offset++) = (sig_y>>3) & 0xff;
nucho 0:77afd7560544 46 *(outbuffer + offset++) = (sig_y>>11) & 0xff;
nucho 0:77afd7560544 47 *(outbuffer + offset++) = ((exp_y<<4) & 0xF0) | ((sig_y>>19)&0x0F);
nucho 0:77afd7560544 48 *(outbuffer + offset++) = (exp_y>>4) & 0x7F;
nucho 0:77afd7560544 49 if(this->y < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 50 long * val_z = (long *) &(this->z);
nucho 0:77afd7560544 51 long exp_z = (((*val_z)>>23)&255);
nucho 0:77afd7560544 52 if(exp_z != 0)
nucho 0:77afd7560544 53 exp_z += 1023-127;
nucho 0:77afd7560544 54 long sig_z = *val_z;
nucho 0:77afd7560544 55 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 56 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 57 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 58 *(outbuffer + offset++) = (sig_z<<5) & 0xff;
nucho 0:77afd7560544 59 *(outbuffer + offset++) = (sig_z>>3) & 0xff;
nucho 0:77afd7560544 60 *(outbuffer + offset++) = (sig_z>>11) & 0xff;
nucho 0:77afd7560544 61 *(outbuffer + offset++) = ((exp_z<<4) & 0xF0) | ((sig_z>>19)&0x0F);
nucho 0:77afd7560544 62 *(outbuffer + offset++) = (exp_z>>4) & 0x7F;
nucho 0:77afd7560544 63 if(this->z < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 64 return offset;
nucho 0:77afd7560544 65 }
nucho 0:77afd7560544 66
nucho 0:77afd7560544 67 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 68 {
nucho 0:77afd7560544 69 int offset = 0;
nucho 0:77afd7560544 70 unsigned long * val_x = (unsigned long*) &(this->x);
nucho 0:77afd7560544 71 offset += 3;
nucho 0:77afd7560544 72 *val_x = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 73 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 74 *val_x |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 75 *val_x |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 76 unsigned long exp_x = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 77 exp_x |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 78 if(exp_x !=0)
nucho 0:77afd7560544 79 *val_x |= ((exp_x)-1023+127)<<23;
nucho 0:77afd7560544 80 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
nucho 0:77afd7560544 81 unsigned long * val_y = (unsigned long*) &(this->y);
nucho 0:77afd7560544 82 offset += 3;
nucho 0:77afd7560544 83 *val_y = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 84 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 85 *val_y |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 86 *val_y |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 87 unsigned long exp_y = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 88 exp_y |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 89 if(exp_y !=0)
nucho 0:77afd7560544 90 *val_y |= ((exp_y)-1023+127)<<23;
nucho 0:77afd7560544 91 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
nucho 0:77afd7560544 92 unsigned long * val_z = (unsigned long*) &(this->z);
nucho 0:77afd7560544 93 offset += 3;
nucho 0:77afd7560544 94 *val_z = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 95 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 96 *val_z |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 97 *val_z |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 98 unsigned long exp_z = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 99 exp_z |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 100 if(exp_z !=0)
nucho 0:77afd7560544 101 *val_z |= ((exp_z)-1023+127)<<23;
nucho 0:77afd7560544 102 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
nucho 0:77afd7560544 103 return offset;
nucho 0:77afd7560544 104 }
nucho 0:77afd7560544 105
nucho 0:77afd7560544 106 virtual const char * getType(){ return "geometry_msgs/Vector3"; };
nucho 0:77afd7560544 107
nucho 0:77afd7560544 108 };
nucho 0:77afd7560544 109
nucho 0:77afd7560544 110 }
nucho 0:77afd7560544 111 #endif