nucho / Mbed 2 deprecated rosserial_mbed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   rosserial_mbed_lib mbed Servo

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