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@3:dff241b66f84, 2011-11-12 (annotated)

Committer:
nucho
Date:
Sat Nov 12 23:53:04 2011 +0000
Revision:
3:dff241b66f84
Parent:
1:098e75fd5ad2 
This program supported the revision of 167 of rosserial.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 3:dff241b66f84 1 #ifndef _ROS_geometry_msgs_Pose2D_h
nucho 3:dff241b66f84 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 3:dff241b66f84 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 3:dff241b66f84 19 virtual int serialize(unsigned char *outbuffer) const
nucho 0:06fc856e99ca 20 {
nucho 0:06fc856e99ca 21 int offset = 0;
nucho 3:dff241b66f84 22 int32_t * val_x = (long *) &(this->x);
nucho 3:dff241b66f84 23 int32_t exp_x = (((*val_x)>>23)&255);
nucho 0:06fc856e99ca 24 if(exp_x != 0)
nucho 0:06fc856e99ca 25 exp_x += 1023-127;
nucho 3:dff241b66f84 26 int32_t 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 3:dff241b66f84 36 int32_t * val_y = (long *) &(this->y);
nucho 3:dff241b66f84 37 int32_t exp_y = (((*val_y)>>23)&255);
nucho 0:06fc856e99ca 38 if(exp_y != 0)
nucho 0:06fc856e99ca 39 exp_y += 1023-127;
nucho 3:dff241b66f84 40 int32_t 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 3:dff241b66f84 50 int32_t * val_theta = (long *) &(this->theta);
nucho 3:dff241b66f84 51 int32_t exp_theta = (((*val_theta)>>23)&255);
nucho 0:06fc856e99ca 52 if(exp_theta != 0)
nucho 0:06fc856e99ca 53 exp_theta += 1023-127;
nucho 3:dff241b66f84 54 int32_t 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 3:dff241b66f84 70 uint32_t * val_x = (uint32_t*) &(this->x);
nucho 0:06fc856e99ca 71 offset += 3;
nucho 3:dff241b66f84 72 *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
nucho 3:dff241b66f84 73 *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 3:dff241b66f84 74 *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 3:dff241b66f84 75 *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 3:dff241b66f84 76 uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
nucho 3:dff241b66f84 77 exp_x |= ((uint32_t)(*(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 3:dff241b66f84 81 uint32_t * val_y = (uint32_t*) &(this->y);
nucho 0:06fc856e99ca 82 offset += 3;
nucho 3:dff241b66f84 83 *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
nucho 3:dff241b66f84 84 *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 3:dff241b66f84 85 *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 3:dff241b66f84 86 *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 3:dff241b66f84 87 uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
nucho 3:dff241b66f84 88 exp_y |= ((uint32_t)(*(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 3:dff241b66f84 92 uint32_t * val_theta = (uint32_t*) &(this->theta);
nucho 0:06fc856e99ca 93 offset += 3;
nucho 3:dff241b66f84 94 *val_theta = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
nucho 3:dff241b66f84 95 *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 3:dff241b66f84 96 *val_theta |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 3:dff241b66f84 97 *val_theta |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 3:dff241b66f84 98 uint32_t exp_theta = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
nucho 3:dff241b66f84 99 exp_theta |= ((uint32_t)(*(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 3:dff241b66f84 106 virtual const char * getType(){ return "geometry_msgs/Pose2D"; };
nucho 3:dff241b66f84 107 virtual const char * getMD5(){ return "938fa65709584ad8e77d238529be13b8"; };
nucho 0:06fc856e99ca 108
nucho 0:06fc856e99ca 109 };
nucho 0:06fc856e99ca 110
nucho 0:06fc856e99ca 111 }
nucho 0:06fc856e99ca 112 #endif