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