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/TwistWithCovariance.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_TwistWithCovariance_h
nucho 1:ff0ec969dad1 2 #define ros_geometry_msgs_TwistWithCovariance_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 #include "geometry_msgs/Twist.h"
nucho 0:77afd7560544 9
nucho 0:77afd7560544 10 namespace geometry_msgs
nucho 0:77afd7560544 11 {
nucho 0:77afd7560544 12
nucho 0:77afd7560544 13 class TwistWithCovariance : public ros::Msg
nucho 0:77afd7560544 14 {
nucho 0:77afd7560544 15 public:
nucho 0:77afd7560544 16 geometry_msgs::Twist twist;
nucho 0:77afd7560544 17 float covariance[36];
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 offset += this->twist.serialize(outbuffer + offset);
nucho 0:77afd7560544 23 unsigned char * covariance_val = (unsigned char *) this->covariance;
nucho 0:77afd7560544 24 for( unsigned char i = 0; i < 36; i++){
nucho 0:77afd7560544 25 long * val_covariancei = (long *) &(this->covariance[i]);
nucho 0:77afd7560544 26 long exp_covariancei = (((*val_covariancei)>>23)&255);
nucho 0:77afd7560544 27 if(exp_covariancei != 0)
nucho 0:77afd7560544 28 exp_covariancei += 1023-127;
nucho 0:77afd7560544 29 long sig_covariancei = *val_covariancei;
nucho 0:77afd7560544 30 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 31 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 32 *(outbuffer + offset++) = 0;
nucho 0:77afd7560544 33 *(outbuffer + offset++) = (sig_covariancei<<5) & 0xff;
nucho 0:77afd7560544 34 *(outbuffer + offset++) = (sig_covariancei>>3) & 0xff;
nucho 0:77afd7560544 35 *(outbuffer + offset++) = (sig_covariancei>>11) & 0xff;
nucho 0:77afd7560544 36 *(outbuffer + offset++) = ((exp_covariancei<<4) & 0xF0) | ((sig_covariancei>>19)&0x0F);
nucho 0:77afd7560544 37 *(outbuffer + offset++) = (exp_covariancei>>4) & 0x7F;
nucho 0:77afd7560544 38 if(this->covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
nucho 0:77afd7560544 39 }
nucho 0:77afd7560544 40 return offset;
nucho 0:77afd7560544 41 }
nucho 0:77afd7560544 42
nucho 0:77afd7560544 43 virtual int deserialize(unsigned char *inbuffer)
nucho 0:77afd7560544 44 {
nucho 0:77afd7560544 45 int offset = 0;
nucho 0:77afd7560544 46 offset += this->twist.deserialize(inbuffer + offset);
nucho 0:77afd7560544 47 unsigned char * covariance_val = (unsigned char *) this->covariance;
nucho 0:77afd7560544 48 for( unsigned char i = 0; i < 36; i++){
nucho 0:77afd7560544 49 unsigned long * val_covariancei = (unsigned long*) &(this->covariance[i]);
nucho 0:77afd7560544 50 offset += 3;
nucho 0:77afd7560544 51 *val_covariancei = ((unsigned long)(*(inbuffer + offset++))>>5 & 0x07);
nucho 0:77afd7560544 52 *val_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 0:77afd7560544 53 *val_covariancei |= ((unsigned long)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 0:77afd7560544 54 *val_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 0:77afd7560544 55 unsigned long exp_covariancei = ((unsigned long)(*(inbuffer + offset++))&0xf0)>>4;
nucho 0:77afd7560544 56 exp_covariancei |= ((unsigned long)(*(inbuffer + offset)) & 0x7f)<<4;
nucho 0:77afd7560544 57 if(exp_covariancei !=0)
nucho 0:77afd7560544 58 *val_covariancei |= ((exp_covariancei)-1023+127)<<23;
nucho 0:77afd7560544 59 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->covariance[i] = -this->covariance[i];
nucho 0:77afd7560544 60 }
nucho 0:77afd7560544 61 return offset;
nucho 0:77afd7560544 62 }
nucho 0:77afd7560544 63
nucho 0:77afd7560544 64 virtual const char * getType(){ return "geometry_msgs/TwistWithCovariance"; };
nucho 0:77afd7560544 65
nucho 0:77afd7560544 66 };
nucho 0:77afd7560544 67
nucho 0:77afd7560544 68 }
nucho 0:77afd7560544 69 #endif