Go to the documentation of this file.00001 #ifndef _ROS_octomap_msgs_Octomap_h
00002 #define _ROS_octomap_msgs_Octomap_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "std_msgs/Header.h"
00009
00010 namespace octomap_msgs
00011 {
00012
00013 class Octomap : public ros::Msg
00014 {
00015 public:
00016 std_msgs::Header header;
00017 bool binary;
00018 const char* id;
00019 float resolution;
00020 uint8_t data_length;
00021 int8_t st_data;
00022 int8_t * data;
00023
00024 virtual int serialize(unsigned char *outbuffer) const
00025 {
00026 int offset = 0;
00027 offset += this->header.serialize(outbuffer + offset);
00028 union {
00029 bool real;
00030 uint8_t base;
00031 } u_binary;
00032 u_binary.real = this->binary;
00033 *(outbuffer + offset + 0) = (u_binary.base >> (8 * 0)) & 0xFF;
00034 offset += sizeof(this->binary);
00035 uint32_t length_id = strlen(this->id);
00036 memcpy(outbuffer + offset, &length_id, sizeof(uint32_t));
00037 offset += 4;
00038 memcpy(outbuffer + offset, this->id, length_id);
00039 offset += length_id;
00040 int32_t * val_resolution = (int32_t *) &(this->resolution);
00041 int32_t exp_resolution = (((*val_resolution)>>23)&255);
00042 if(exp_resolution != 0)
00043 exp_resolution += 1023-127;
00044 int32_t sig_resolution = *val_resolution;
00045 *(outbuffer + offset++) = 0;
00046 *(outbuffer + offset++) = 0;
00047 *(outbuffer + offset++) = 0;
00048 *(outbuffer + offset++) = (sig_resolution<<5) & 0xff;
00049 *(outbuffer + offset++) = (sig_resolution>>3) & 0xff;
00050 *(outbuffer + offset++) = (sig_resolution>>11) & 0xff;
00051 *(outbuffer + offset++) = ((exp_resolution<<4) & 0xF0) | ((sig_resolution>>19)&0x0F);
00052 *(outbuffer + offset++) = (exp_resolution>>4) & 0x7F;
00053 if(this->resolution < 0) *(outbuffer + offset -1) |= 0x80;
00054 *(outbuffer + offset++) = data_length;
00055 *(outbuffer + offset++) = 0;
00056 *(outbuffer + offset++) = 0;
00057 *(outbuffer + offset++) = 0;
00058 for( uint8_t i = 0; i < data_length; i++){
00059 union {
00060 int8_t real;
00061 uint8_t base;
00062 } u_datai;
00063 u_datai.real = this->data[i];
00064 *(outbuffer + offset + 0) = (u_datai.base >> (8 * 0)) & 0xFF;
00065 offset += sizeof(this->data[i]);
00066 }
00067 return offset;
00068 }
00069
00070 virtual int deserialize(unsigned char *inbuffer)
00071 {
00072 int offset = 0;
00073 offset += this->header.deserialize(inbuffer + offset);
00074 union {
00075 bool real;
00076 uint8_t base;
00077 } u_binary;
00078 u_binary.base = 0;
00079 u_binary.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00080 this->binary = u_binary.real;
00081 offset += sizeof(this->binary);
00082 uint32_t length_id;
00083 memcpy(&length_id, (inbuffer + offset), sizeof(uint32_t));
00084 offset += 4;
00085 for(unsigned int k= offset; k< offset+length_id; ++k){
00086 inbuffer[k-1]=inbuffer[k];
00087 }
00088 inbuffer[offset+length_id-1]=0;
00089 this->id = (char *)(inbuffer + offset-1);
00090 offset += length_id;
00091 uint32_t * val_resolution = (uint32_t*) &(this->resolution);
00092 offset += 3;
00093 *val_resolution = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00094 *val_resolution |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00095 *val_resolution |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00096 *val_resolution |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00097 uint32_t exp_resolution = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00098 exp_resolution |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00099 if(exp_resolution !=0)
00100 *val_resolution |= ((exp_resolution)-1023+127)<<23;
00101 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->resolution = -this->resolution;
00102 uint8_t data_lengthT = *(inbuffer + offset++);
00103 if(data_lengthT > data_length)
00104 this->data = (int8_t*)realloc(this->data, data_lengthT * sizeof(int8_t));
00105 offset += 3;
00106 data_length = data_lengthT;
00107 for( uint8_t i = 0; i < data_length; i++){
00108 union {
00109 int8_t real;
00110 uint8_t base;
00111 } u_st_data;
00112 u_st_data.base = 0;
00113 u_st_data.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00114 this->st_data = u_st_data.real;
00115 offset += sizeof(this->st_data);
00116 memcpy( &(this->data[i]), &(this->st_data), sizeof(int8_t));
00117 }
00118 return offset;
00119 }
00120
00121 const char * getType(){ return "octomap_msgs/Octomap"; };
00122 const char * getMD5(){ return "9a45536b45c5e409cd49f04bb2d9999f"; };
00123
00124 };
00125
00126 }
00127 #endif