nucho / Mbed 2 deprecated rosserial_mbed

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

Dependencies:   rosserial_mbed_lib mbed Servo

geometry_msgs/Pose2D.h@1:098e75fd5ad2, 2011-10-16 (annotated)

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