modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

Committer:
jjzak
Date:
Sat Oct 26 15:39:01 2013 +0000
Revision:
6:3c54bc7badd4
modify for Hydro version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjzak 6:3c54bc7badd4 1 #ifndef _ROS_sensor_msgs_CameraInfo_h
jjzak 6:3c54bc7badd4 2 #define _ROS_sensor_msgs_CameraInfo_h
jjzak 6:3c54bc7badd4 3
jjzak 6:3c54bc7badd4 4 #include <stdint.h>
jjzak 6:3c54bc7badd4 5 #include <string.h>
jjzak 6:3c54bc7badd4 6 #include <stdlib.h>
jjzak 6:3c54bc7badd4 7 #include "ros/msg.h"
jjzak 6:3c54bc7badd4 8 #include "std_msgs/Header.h"
jjzak 6:3c54bc7badd4 9 #include "sensor_msgs/RegionOfInterest.h"
jjzak 6:3c54bc7badd4 10
jjzak 6:3c54bc7badd4 11 namespace sensor_msgs
jjzak 6:3c54bc7badd4 12 {
jjzak 6:3c54bc7badd4 13
jjzak 6:3c54bc7badd4 14 class CameraInfo : public ros::Msg
jjzak 6:3c54bc7badd4 15 {
jjzak 6:3c54bc7badd4 16 public:
jjzak 6:3c54bc7badd4 17 std_msgs::Header header;
jjzak 6:3c54bc7badd4 18 uint32_t height;
jjzak 6:3c54bc7badd4 19 uint32_t width;
jjzak 6:3c54bc7badd4 20 char * distortion_model;
jjzak 6:3c54bc7badd4 21 uint8_t D_length;
jjzak 6:3c54bc7badd4 22 float st_D;
jjzak 6:3c54bc7badd4 23 float * D;
jjzak 6:3c54bc7badd4 24 float K[9];
jjzak 6:3c54bc7badd4 25 float R[9];
jjzak 6:3c54bc7badd4 26 float P[12];
jjzak 6:3c54bc7badd4 27 uint32_t binning_x;
jjzak 6:3c54bc7badd4 28 uint32_t binning_y;
jjzak 6:3c54bc7badd4 29 sensor_msgs::RegionOfInterest roi;
jjzak 6:3c54bc7badd4 30
jjzak 6:3c54bc7badd4 31 virtual int serialize(unsigned char *outbuffer) const
jjzak 6:3c54bc7badd4 32 {
jjzak 6:3c54bc7badd4 33 int offset = 0;
jjzak 6:3c54bc7badd4 34 offset += this->header.serialize(outbuffer + offset);
jjzak 6:3c54bc7badd4 35 *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
jjzak 6:3c54bc7badd4 36 *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
jjzak 6:3c54bc7badd4 37 *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
jjzak 6:3c54bc7badd4 38 *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
jjzak 6:3c54bc7badd4 39 offset += sizeof(this->height);
jjzak 6:3c54bc7badd4 40 *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
jjzak 6:3c54bc7badd4 41 *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
jjzak 6:3c54bc7badd4 42 *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
jjzak 6:3c54bc7badd4 43 *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
jjzak 6:3c54bc7badd4 44 offset += sizeof(this->width);
jjzak 6:3c54bc7badd4 45 uint32_t length_distortion_model = strlen( (const char*) this->distortion_model);
jjzak 6:3c54bc7badd4 46 memcpy(outbuffer + offset, &length_distortion_model, sizeof(uint32_t));
jjzak 6:3c54bc7badd4 47 offset += 4;
jjzak 6:3c54bc7badd4 48 memcpy(outbuffer + offset, this->distortion_model, length_distortion_model);
jjzak 6:3c54bc7badd4 49 offset += length_distortion_model;
jjzak 6:3c54bc7badd4 50 *(outbuffer + offset++) = D_length;
jjzak 6:3c54bc7badd4 51 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 52 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 53 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 54 for( uint8_t i = 0; i < D_length; i++){
jjzak 6:3c54bc7badd4 55 int32_t * val_Di = (int32_t *) &(this->D[i]);
jjzak 6:3c54bc7badd4 56 int32_t exp_Di = (((*val_Di)>>23)&255);
jjzak 6:3c54bc7badd4 57 if(exp_Di != 0)
jjzak 6:3c54bc7badd4 58 exp_Di += 1023-127;
jjzak 6:3c54bc7badd4 59 int32_t sig_Di = *val_Di;
jjzak 6:3c54bc7badd4 60 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 61 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 62 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 63 *(outbuffer + offset++) = (sig_Di<<5) & 0xff;
jjzak 6:3c54bc7badd4 64 *(outbuffer + offset++) = (sig_Di>>3) & 0xff;
jjzak 6:3c54bc7badd4 65 *(outbuffer + offset++) = (sig_Di>>11) & 0xff;
jjzak 6:3c54bc7badd4 66 *(outbuffer + offset++) = ((exp_Di<<4) & 0xF0) | ((sig_Di>>19)&0x0F);
jjzak 6:3c54bc7badd4 67 *(outbuffer + offset++) = (exp_Di>>4) & 0x7F;
jjzak 6:3c54bc7badd4 68 if(this->D[i] < 0) *(outbuffer + offset -1) |= 0x80;
jjzak 6:3c54bc7badd4 69 }
jjzak 6:3c54bc7badd4 70 unsigned char * K_val = (unsigned char *) this->K;
jjzak 6:3c54bc7badd4 71 for( uint8_t i = 0; i < 9; i++){
jjzak 6:3c54bc7badd4 72 int32_t * val_Ki = (int32_t *) &(this->K[i]);
jjzak 6:3c54bc7badd4 73 int32_t exp_Ki = (((*val_Ki)>>23)&255);
jjzak 6:3c54bc7badd4 74 if(exp_Ki != 0)
jjzak 6:3c54bc7badd4 75 exp_Ki += 1023-127;
jjzak 6:3c54bc7badd4 76 int32_t sig_Ki = *val_Ki;
jjzak 6:3c54bc7badd4 77 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 78 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 79 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 80 *(outbuffer + offset++) = (sig_Ki<<5) & 0xff;
jjzak 6:3c54bc7badd4 81 *(outbuffer + offset++) = (sig_Ki>>3) & 0xff;
jjzak 6:3c54bc7badd4 82 *(outbuffer + offset++) = (sig_Ki>>11) & 0xff;
jjzak 6:3c54bc7badd4 83 *(outbuffer + offset++) = ((exp_Ki<<4) & 0xF0) | ((sig_Ki>>19)&0x0F);
jjzak 6:3c54bc7badd4 84 *(outbuffer + offset++) = (exp_Ki>>4) & 0x7F;
jjzak 6:3c54bc7badd4 85 if(this->K[i] < 0) *(outbuffer + offset -1) |= 0x80;
jjzak 6:3c54bc7badd4 86 }
jjzak 6:3c54bc7badd4 87 unsigned char * R_val = (unsigned char *) this->R;
jjzak 6:3c54bc7badd4 88 for( uint8_t i = 0; i < 9; i++){
jjzak 6:3c54bc7badd4 89 int32_t * val_Ri = (int32_t *) &(this->R[i]);
jjzak 6:3c54bc7badd4 90 int32_t exp_Ri = (((*val_Ri)>>23)&255);
jjzak 6:3c54bc7badd4 91 if(exp_Ri != 0)
jjzak 6:3c54bc7badd4 92 exp_Ri += 1023-127;
jjzak 6:3c54bc7badd4 93 int32_t sig_Ri = *val_Ri;
jjzak 6:3c54bc7badd4 94 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 95 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 96 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 97 *(outbuffer + offset++) = (sig_Ri<<5) & 0xff;
jjzak 6:3c54bc7badd4 98 *(outbuffer + offset++) = (sig_Ri>>3) & 0xff;
jjzak 6:3c54bc7badd4 99 *(outbuffer + offset++) = (sig_Ri>>11) & 0xff;
jjzak 6:3c54bc7badd4 100 *(outbuffer + offset++) = ((exp_Ri<<4) & 0xF0) | ((sig_Ri>>19)&0x0F);
jjzak 6:3c54bc7badd4 101 *(outbuffer + offset++) = (exp_Ri>>4) & 0x7F;
jjzak 6:3c54bc7badd4 102 if(this->R[i] < 0) *(outbuffer + offset -1) |= 0x80;
jjzak 6:3c54bc7badd4 103 }
jjzak 6:3c54bc7badd4 104 unsigned char * P_val = (unsigned char *) this->P;
jjzak 6:3c54bc7badd4 105 for( uint8_t i = 0; i < 12; i++){
jjzak 6:3c54bc7badd4 106 int32_t * val_Pi = (int32_t *) &(this->P[i]);
jjzak 6:3c54bc7badd4 107 int32_t exp_Pi = (((*val_Pi)>>23)&255);
jjzak 6:3c54bc7badd4 108 if(exp_Pi != 0)
jjzak 6:3c54bc7badd4 109 exp_Pi += 1023-127;
jjzak 6:3c54bc7badd4 110 int32_t sig_Pi = *val_Pi;
jjzak 6:3c54bc7badd4 111 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 112 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 113 *(outbuffer + offset++) = 0;
jjzak 6:3c54bc7badd4 114 *(outbuffer + offset++) = (sig_Pi<<5) & 0xff;
jjzak 6:3c54bc7badd4 115 *(outbuffer + offset++) = (sig_Pi>>3) & 0xff;
jjzak 6:3c54bc7badd4 116 *(outbuffer + offset++) = (sig_Pi>>11) & 0xff;
jjzak 6:3c54bc7badd4 117 *(outbuffer + offset++) = ((exp_Pi<<4) & 0xF0) | ((sig_Pi>>19)&0x0F);
jjzak 6:3c54bc7badd4 118 *(outbuffer + offset++) = (exp_Pi>>4) & 0x7F;
jjzak 6:3c54bc7badd4 119 if(this->P[i] < 0) *(outbuffer + offset -1) |= 0x80;
jjzak 6:3c54bc7badd4 120 }
jjzak 6:3c54bc7badd4 121 *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF;
jjzak 6:3c54bc7badd4 122 *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF;
jjzak 6:3c54bc7badd4 123 *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF;
jjzak 6:3c54bc7badd4 124 *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF;
jjzak 6:3c54bc7badd4 125 offset += sizeof(this->binning_x);
jjzak 6:3c54bc7badd4 126 *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF;
jjzak 6:3c54bc7badd4 127 *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF;
jjzak 6:3c54bc7badd4 128 *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF;
jjzak 6:3c54bc7badd4 129 *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF;
jjzak 6:3c54bc7badd4 130 offset += sizeof(this->binning_y);
jjzak 6:3c54bc7badd4 131 offset += this->roi.serialize(outbuffer + offset);
jjzak 6:3c54bc7badd4 132 return offset;
jjzak 6:3c54bc7badd4 133 }
jjzak 6:3c54bc7badd4 134
jjzak 6:3c54bc7badd4 135 virtual int deserialize(unsigned char *inbuffer)
jjzak 6:3c54bc7badd4 136 {
jjzak 6:3c54bc7badd4 137 int offset = 0;
jjzak 6:3c54bc7badd4 138 offset += this->header.deserialize(inbuffer + offset);
jjzak 6:3c54bc7badd4 139 this->height = ((uint32_t) (*(inbuffer + offset)));
jjzak 6:3c54bc7badd4 140 this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
jjzak 6:3c54bc7badd4 141 this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
jjzak 6:3c54bc7badd4 142 this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
jjzak 6:3c54bc7badd4 143 offset += sizeof(this->height);
jjzak 6:3c54bc7badd4 144 this->width = ((uint32_t) (*(inbuffer + offset)));
jjzak 6:3c54bc7badd4 145 this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
jjzak 6:3c54bc7badd4 146 this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
jjzak 6:3c54bc7badd4 147 this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
jjzak 6:3c54bc7badd4 148 offset += sizeof(this->width);
jjzak 6:3c54bc7badd4 149 uint32_t length_distortion_model;
jjzak 6:3c54bc7badd4 150 memcpy(&length_distortion_model, (inbuffer + offset), sizeof(uint32_t));
jjzak 6:3c54bc7badd4 151 offset += 4;
jjzak 6:3c54bc7badd4 152 for(unsigned int k= offset; k< offset+length_distortion_model; ++k){
jjzak 6:3c54bc7badd4 153 inbuffer[k-1]=inbuffer[k];
jjzak 6:3c54bc7badd4 154 }
jjzak 6:3c54bc7badd4 155 inbuffer[offset+length_distortion_model-1]=0;
jjzak 6:3c54bc7badd4 156 this->distortion_model = (char *)(inbuffer + offset-1);
jjzak 6:3c54bc7badd4 157 offset += length_distortion_model;
jjzak 6:3c54bc7badd4 158 uint8_t D_lengthT = *(inbuffer + offset++);
jjzak 6:3c54bc7badd4 159 if(D_lengthT > D_length)
jjzak 6:3c54bc7badd4 160 this->D = (float*)realloc(this->D, D_lengthT * sizeof(float));
jjzak 6:3c54bc7badd4 161 offset += 3;
jjzak 6:3c54bc7badd4 162 D_length = D_lengthT;
jjzak 6:3c54bc7badd4 163 for( uint8_t i = 0; i < D_length; i++){
jjzak 6:3c54bc7badd4 164 uint32_t * val_st_D = (uint32_t*) &(this->st_D);
jjzak 6:3c54bc7badd4 165 offset += 3;
jjzak 6:3c54bc7badd4 166 *val_st_D = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
jjzak 6:3c54bc7badd4 167 *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
jjzak 6:3c54bc7badd4 168 *val_st_D |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
jjzak 6:3c54bc7badd4 169 *val_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
jjzak 6:3c54bc7badd4 170 uint32_t exp_st_D = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
jjzak 6:3c54bc7badd4 171 exp_st_D |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
jjzak 6:3c54bc7badd4 172 if(exp_st_D !=0)
jjzak 6:3c54bc7badd4 173 *val_st_D |= ((exp_st_D)-1023+127)<<23;
jjzak 6:3c54bc7badd4 174 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_D = -this->st_D;
jjzak 6:3c54bc7badd4 175 memcpy( &(this->D[i]), &(this->st_D), sizeof(float));
jjzak 6:3c54bc7badd4 176 }
jjzak 6:3c54bc7badd4 177 uint8_t * K_val = (uint8_t*) this->K;
jjzak 6:3c54bc7badd4 178 for( uint8_t i = 0; i < 9; i++){
jjzak 6:3c54bc7badd4 179 uint32_t * val_Ki = (uint32_t*) &(this->K[i]);
jjzak 6:3c54bc7badd4 180 offset += 3;
jjzak 6:3c54bc7badd4 181 *val_Ki = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
jjzak 6:3c54bc7badd4 182 *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
jjzak 6:3c54bc7badd4 183 *val_Ki |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
jjzak 6:3c54bc7badd4 184 *val_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
jjzak 6:3c54bc7badd4 185 uint32_t exp_Ki = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
jjzak 6:3c54bc7badd4 186 exp_Ki |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
jjzak 6:3c54bc7badd4 187 if(exp_Ki !=0)
jjzak 6:3c54bc7badd4 188 *val_Ki |= ((exp_Ki)-1023+127)<<23;
jjzak 6:3c54bc7badd4 189 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->K[i] = -this->K[i];
jjzak 6:3c54bc7badd4 190 }
jjzak 6:3c54bc7badd4 191 uint8_t * R_val = (uint8_t*) this->R;
jjzak 6:3c54bc7badd4 192 for( uint8_t i = 0; i < 9; i++){
jjzak 6:3c54bc7badd4 193 uint32_t * val_Ri = (uint32_t*) &(this->R[i]);
jjzak 6:3c54bc7badd4 194 offset += 3;
jjzak 6:3c54bc7badd4 195 *val_Ri = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
jjzak 6:3c54bc7badd4 196 *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
jjzak 6:3c54bc7badd4 197 *val_Ri |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
jjzak 6:3c54bc7badd4 198 *val_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
jjzak 6:3c54bc7badd4 199 uint32_t exp_Ri = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
jjzak 6:3c54bc7badd4 200 exp_Ri |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
jjzak 6:3c54bc7badd4 201 if(exp_Ri !=0)
jjzak 6:3c54bc7badd4 202 *val_Ri |= ((exp_Ri)-1023+127)<<23;
jjzak 6:3c54bc7badd4 203 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->R[i] = -this->R[i];
jjzak 6:3c54bc7badd4 204 }
jjzak 6:3c54bc7badd4 205 uint8_t * P_val = (uint8_t*) this->P;
jjzak 6:3c54bc7badd4 206 for( uint8_t i = 0; i < 12; i++){
jjzak 6:3c54bc7badd4 207 uint32_t * val_Pi = (uint32_t*) &(this->P[i]);
jjzak 6:3c54bc7badd4 208 offset += 3;
jjzak 6:3c54bc7badd4 209 *val_Pi = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
jjzak 6:3c54bc7badd4 210 *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
jjzak 6:3c54bc7badd4 211 *val_Pi |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
jjzak 6:3c54bc7badd4 212 *val_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
jjzak 6:3c54bc7badd4 213 uint32_t exp_Pi = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
jjzak 6:3c54bc7badd4 214 exp_Pi |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
jjzak 6:3c54bc7badd4 215 if(exp_Pi !=0)
jjzak 6:3c54bc7badd4 216 *val_Pi |= ((exp_Pi)-1023+127)<<23;
jjzak 6:3c54bc7badd4 217 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->P[i] = -this->P[i];
jjzak 6:3c54bc7badd4 218 }
jjzak 6:3c54bc7badd4 219 this->binning_x = ((uint32_t) (*(inbuffer + offset)));
jjzak 6:3c54bc7badd4 220 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
jjzak 6:3c54bc7badd4 221 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
jjzak 6:3c54bc7badd4 222 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
jjzak 6:3c54bc7badd4 223 offset += sizeof(this->binning_x);
jjzak 6:3c54bc7badd4 224 this->binning_y = ((uint32_t) (*(inbuffer + offset)));
jjzak 6:3c54bc7badd4 225 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
jjzak 6:3c54bc7badd4 226 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
jjzak 6:3c54bc7badd4 227 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
jjzak 6:3c54bc7badd4 228 offset += sizeof(this->binning_y);
jjzak 6:3c54bc7badd4 229 offset += this->roi.deserialize(inbuffer + offset);
jjzak 6:3c54bc7badd4 230 return offset;
jjzak 6:3c54bc7badd4 231 }
jjzak 6:3c54bc7badd4 232
jjzak 6:3c54bc7badd4 233 const char * getType(){ return "sensor_msgs/CameraInfo"; };
jjzak 6:3c54bc7badd4 234 const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; };
jjzak 6:3c54bc7badd4 235
jjzak 6:3c54bc7badd4 236 };
jjzak 6:3c54bc7badd4 237
jjzak 6:3c54bc7badd4 238 }
jjzak 6:3c54bc7badd4 239 #endif