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@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_TwistWithCovariance_h
nucho 3:dff241b66f84 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 3:dff241b66f84 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 3:dff241b66f84 19 virtual int serialize(unsigned char *outbuffer) const
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 3:dff241b66f84 24 for( uint8_t i = 0; i < 36; i++){
nucho 3:dff241b66f84 25 int32_t * val_covariancei = (long *) &(this->covariance[i]);
nucho 3:dff241b66f84 26 int32_t exp_covariancei = (((*val_covariancei)>>23)&255);
nucho 0:06fc856e99ca 27 if(exp_covariancei != 0)
nucho 0:06fc856e99ca 28 exp_covariancei += 1023-127;
nucho 3:dff241b66f84 29 int32_t 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 3:dff241b66f84 47 uint8_t * covariance_val = (uint8_t*) this->covariance;
nucho 3:dff241b66f84 48 for( uint8_t i = 0; i < 36; i++){
nucho 3:dff241b66f84 49 uint32_t * val_covariancei = (uint32_t*) &(this->covariance[i]);
nucho 0:06fc856e99ca 50 offset += 3;
nucho 3:dff241b66f84 51 *val_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
nucho 3:dff241b66f84 52 *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
nucho 3:dff241b66f84 53 *val_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
nucho 3:dff241b66f84 54 *val_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
nucho 3:dff241b66f84 55 uint32_t exp_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
nucho 3:dff241b66f84 56 exp_covariancei |= ((uint32_t)(*(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 3:dff241b66f84 64 virtual const char * getType(){ return "geometry_msgs/TwistWithCovariance"; };
nucho 3:dff241b66f84 65 virtual const char * getMD5(){ return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; };
nucho 0:06fc856e99ca 66
nucho 0:06fc856e99ca 67 };
nucho 0:06fc856e99ca 68
nucho 0:06fc856e99ca 69 }
nucho 0:06fc856e99ca 70 #endif