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