Gary Servin
ROS Serial library for Mbed platforms for ROS Indigo Igloo. Check http://wiki.ros.org/rosserial_mbed/ for more information
Dependents: rosserial_mbed_hello_world_publisher rtos_base_control rosserial_mbed_F64MA ROS-RTOS ... more
Embed:
(wiki syntax)
Show/hide line numbers
CameraInfo.h
00001 #ifndef _ROS_sensor_msgs_CameraInfo_h 00002 #define _ROS_sensor_msgs_CameraInfo_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 #include "sensor_msgs/RegionOfInterest.h" 00010 00011 namespace sensor_msgs 00012 { 00013 00014 class CameraInfo : public ros::Msg 00015 { 00016 public: 00017 std_msgs::Header header; 00018 uint32_t height; 00019 uint32_t width; 00020 const char* distortion_model; 00021 uint8_t D_length; 00022 double st_D; 00023 double * D; 00024 double K[9]; 00025 double R[9]; 00026 double P[12]; 00027 uint32_t binning_x; 00028 uint32_t binning_y; 00029 sensor_msgs::RegionOfInterest roi; 00030 00031 CameraInfo(): 00032 header(), 00033 height(0), 00034 width(0), 00035 distortion_model(""), 00036 D_length(0), D(NULL), 00037 K(), 00038 R(), 00039 P(), 00040 binning_x(0), 00041 binning_y(0), 00042 roi() 00043 { 00044 } 00045 00046 virtual int serialize(unsigned char *outbuffer) const 00047 { 00048 int offset = 0; 00049 offset += this->header.serialize(outbuffer + offset); 00050 *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF; 00051 *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF; 00052 *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF; 00053 *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF; 00054 offset += sizeof(this->height); 00055 *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF; 00056 *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF; 00057 *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF; 00058 *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF; 00059 offset += sizeof(this->width); 00060 uint32_t length_distortion_model = strlen(this->distortion_model); 00061 memcpy(outbuffer + offset, &length_distortion_model, sizeof(uint32_t)); 00062 offset += 4; 00063 memcpy(outbuffer + offset, this->distortion_model, length_distortion_model); 00064 offset += length_distortion_model; 00065 *(outbuffer + offset++) = D_length; 00066 *(outbuffer + offset++) = 0; 00067 *(outbuffer + offset++) = 0; 00068 *(outbuffer + offset++) = 0; 00069 for( uint8_t i = 0; i < D_length; i++){ 00070 union { 00071 double real; 00072 uint64_t base; 00073 } u_Di; 00074 u_Di.real = this->D[i]; 00075 *(outbuffer + offset + 0) = (u_Di.base >> (8 * 0)) & 0xFF; 00076 *(outbuffer + offset + 1) = (u_Di.base >> (8 * 1)) & 0xFF; 00077 *(outbuffer + offset + 2) = (u_Di.base >> (8 * 2)) & 0xFF; 00078 *(outbuffer + offset + 3) = (u_Di.base >> (8 * 3)) & 0xFF; 00079 *(outbuffer + offset + 4) = (u_Di.base >> (8 * 4)) & 0xFF; 00080 *(outbuffer + offset + 5) = (u_Di.base >> (8 * 5)) & 0xFF; 00081 *(outbuffer + offset + 6) = (u_Di.base >> (8 * 6)) & 0xFF; 00082 *(outbuffer + offset + 7) = (u_Di.base >> (8 * 7)) & 0xFF; 00083 offset += sizeof(this->D[i]); 00084 } 00085 for( uint8_t i = 0; i < 9; i++){ 00086 union { 00087 double real; 00088 uint64_t base; 00089 } u_Ki; 00090 u_Ki.real = this->K[i]; 00091 *(outbuffer + offset + 0) = (u_Ki.base >> (8 * 0)) & 0xFF; 00092 *(outbuffer + offset + 1) = (u_Ki.base >> (8 * 1)) & 0xFF; 00093 *(outbuffer + offset + 2) = (u_Ki.base >> (8 * 2)) & 0xFF; 00094 *(outbuffer + offset + 3) = (u_Ki.base >> (8 * 3)) & 0xFF; 00095 *(outbuffer + offset + 4) = (u_Ki.base >> (8 * 4)) & 0xFF; 00096 *(outbuffer + offset + 5) = (u_Ki.base >> (8 * 5)) & 0xFF; 00097 *(outbuffer + offset + 6) = (u_Ki.base >> (8 * 6)) & 0xFF; 00098 *(outbuffer + offset + 7) = (u_Ki.base >> (8 * 7)) & 0xFF; 00099 offset += sizeof(this->K[i]); 00100 } 00101 for( uint8_t i = 0; i < 9; i++){ 00102 union { 00103 double real; 00104 uint64_t base; 00105 } u_Ri; 00106 u_Ri.real = this->R[i]; 00107 *(outbuffer + offset + 0) = (u_Ri.base >> (8 * 0)) & 0xFF; 00108 *(outbuffer + offset + 1) = (u_Ri.base >> (8 * 1)) & 0xFF; 00109 *(outbuffer + offset + 2) = (u_Ri.base >> (8 * 2)) & 0xFF; 00110 *(outbuffer + offset + 3) = (u_Ri.base >> (8 * 3)) & 0xFF; 00111 *(outbuffer + offset + 4) = (u_Ri.base >> (8 * 4)) & 0xFF; 00112 *(outbuffer + offset + 5) = (u_Ri.base >> (8 * 5)) & 0xFF; 00113 *(outbuffer + offset + 6) = (u_Ri.base >> (8 * 6)) & 0xFF; 00114 *(outbuffer + offset + 7) = (u_Ri.base >> (8 * 7)) & 0xFF; 00115 offset += sizeof(this->R[i]); 00116 } 00117 for( uint8_t i = 0; i < 12; i++){ 00118 union { 00119 double real; 00120 uint64_t base; 00121 } u_Pi; 00122 u_Pi.real = this->P[i]; 00123 *(outbuffer + offset + 0) = (u_Pi.base >> (8 * 0)) & 0xFF; 00124 *(outbuffer + offset + 1) = (u_Pi.base >> (8 * 1)) & 0xFF; 00125 *(outbuffer + offset + 2) = (u_Pi.base >> (8 * 2)) & 0xFF; 00126 *(outbuffer + offset + 3) = (u_Pi.base >> (8 * 3)) & 0xFF; 00127 *(outbuffer + offset + 4) = (u_Pi.base >> (8 * 4)) & 0xFF; 00128 *(outbuffer + offset + 5) = (u_Pi.base >> (8 * 5)) & 0xFF; 00129 *(outbuffer + offset + 6) = (u_Pi.base >> (8 * 6)) & 0xFF; 00130 *(outbuffer + offset + 7) = (u_Pi.base >> (8 * 7)) & 0xFF; 00131 offset += sizeof(this->P[i]); 00132 } 00133 *(outbuffer + offset + 0) = (this->binning_x >> (8 * 0)) & 0xFF; 00134 *(outbuffer + offset + 1) = (this->binning_x >> (8 * 1)) & 0xFF; 00135 *(outbuffer + offset + 2) = (this->binning_x >> (8 * 2)) & 0xFF; 00136 *(outbuffer + offset + 3) = (this->binning_x >> (8 * 3)) & 0xFF; 00137 offset += sizeof(this->binning_x); 00138 *(outbuffer + offset + 0) = (this->binning_y >> (8 * 0)) & 0xFF; 00139 *(outbuffer + offset + 1) = (this->binning_y >> (8 * 1)) & 0xFF; 00140 *(outbuffer + offset + 2) = (this->binning_y >> (8 * 2)) & 0xFF; 00141 *(outbuffer + offset + 3) = (this->binning_y >> (8 * 3)) & 0xFF; 00142 offset += sizeof(this->binning_y); 00143 offset += this->roi.serialize(outbuffer + offset); 00144 return offset; 00145 } 00146 00147 virtual int deserialize(unsigned char *inbuffer) 00148 { 00149 int offset = 0; 00150 offset += this->header.deserialize(inbuffer + offset); 00151 this->height = ((uint32_t) (*(inbuffer + offset))); 00152 this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00153 this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00154 this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00155 offset += sizeof(this->height); 00156 this->width = ((uint32_t) (*(inbuffer + offset))); 00157 this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00158 this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00159 this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00160 offset += sizeof(this->width); 00161 uint32_t length_distortion_model; 00162 memcpy(&length_distortion_model, (inbuffer + offset), sizeof(uint32_t)); 00163 offset += 4; 00164 for(unsigned int k= offset; k< offset+length_distortion_model; ++k){ 00165 inbuffer[k-1]=inbuffer[k]; 00166 } 00167 inbuffer[offset+length_distortion_model-1]=0; 00168 this->distortion_model = (char *)(inbuffer + offset-1); 00169 offset += length_distortion_model; 00170 uint8_t D_lengthT = *(inbuffer + offset++); 00171 if(D_lengthT > D_length) 00172 this->D = (double*)realloc(this->D, D_lengthT * sizeof(double)); 00173 offset += 3; 00174 D_length = D_lengthT; 00175 for( uint8_t i = 0; i < D_length; i++){ 00176 union { 00177 double real; 00178 uint64_t base; 00179 } u_st_D; 00180 u_st_D.base = 0; 00181 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00182 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00183 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00184 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00185 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00186 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00187 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00188 u_st_D.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00189 this->st_D = u_st_D.real; 00190 offset += sizeof(this->st_D); 00191 memcpy( &(this->D[i]), &(this->st_D), sizeof(double)); 00192 } 00193 for( uint8_t i = 0; i < 9; i++){ 00194 union { 00195 double real; 00196 uint64_t base; 00197 } u_Ki; 00198 u_Ki.base = 0; 00199 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00200 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00201 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00202 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00203 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00204 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00205 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00206 u_Ki.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00207 this->K[i] = u_Ki.real; 00208 offset += sizeof(this->K[i]); 00209 } 00210 for( uint8_t i = 0; i < 9; i++){ 00211 union { 00212 double real; 00213 uint64_t base; 00214 } u_Ri; 00215 u_Ri.base = 0; 00216 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00217 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00218 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00219 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00220 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00221 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00222 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00223 u_Ri.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00224 this->R[i] = u_Ri.real; 00225 offset += sizeof(this->R[i]); 00226 } 00227 for( uint8_t i = 0; i < 12; i++){ 00228 union { 00229 double real; 00230 uint64_t base; 00231 } u_Pi; 00232 u_Pi.base = 0; 00233 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00234 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00235 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00236 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00237 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00238 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00239 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00240 u_Pi.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00241 this->P[i] = u_Pi.real; 00242 offset += sizeof(this->P[i]); 00243 } 00244 this->binning_x = ((uint32_t) (*(inbuffer + offset))); 00245 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00246 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00247 this->binning_x |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00248 offset += sizeof(this->binning_x); 00249 this->binning_y = ((uint32_t) (*(inbuffer + offset))); 00250 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00251 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00252 this->binning_y |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00253 offset += sizeof(this->binning_y); 00254 offset += this->roi.deserialize(inbuffer + offset); 00255 return offset; 00256 } 00257 00258 const char * getType(){ return "sensor_msgs/CameraInfo"; }; 00259 const char * getMD5(){ return "c9a58c1b0b154e0e6da7578cb991d214"; }; 00260 00261 }; 00262 00263 } 00264 #endif
Generated on Tue Jul 12 2022 18:39:38 by
1.7.2