Working towards recieving twists
Fork of ros_lib_kinetic by
Embed:
(wiki syntax)
Show/hide line numbers
LaserScan.h
00001 #ifndef _ROS_sensor_msgs_LaserScan_h 00002 #define _ROS_sensor_msgs_LaserScan_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 sensor_msgs 00011 { 00012 00013 class LaserScan : public ros::Msg 00014 { 00015 public: 00016 typedef std_msgs::Header _header_type; 00017 _header_type header; 00018 typedef float _angle_min_type; 00019 _angle_min_type angle_min; 00020 typedef float _angle_max_type; 00021 _angle_max_type angle_max; 00022 typedef float _angle_increment_type; 00023 _angle_increment_type angle_increment; 00024 typedef float _time_increment_type; 00025 _time_increment_type time_increment; 00026 typedef float _scan_time_type; 00027 _scan_time_type scan_time; 00028 typedef float _range_min_type; 00029 _range_min_type range_min; 00030 typedef float _range_max_type; 00031 _range_max_type range_max; 00032 uint32_t ranges_length; 00033 typedef float _ranges_type; 00034 _ranges_type st_ranges; 00035 _ranges_type * ranges; 00036 uint32_t intensities_length; 00037 typedef float _intensities_type; 00038 _intensities_type st_intensities; 00039 _intensities_type * intensities; 00040 00041 LaserScan(): 00042 header(), 00043 angle_min(0), 00044 angle_max(0), 00045 angle_increment(0), 00046 time_increment(0), 00047 scan_time(0), 00048 range_min(0), 00049 range_max(0), 00050 ranges_length(0), ranges(NULL), 00051 intensities_length(0), intensities(NULL) 00052 { 00053 } 00054 00055 virtual int serialize(unsigned char *outbuffer) const 00056 { 00057 int offset = 0; 00058 offset += this->header.serialize(outbuffer + offset); 00059 union { 00060 float real; 00061 uint32_t base; 00062 } u_angle_min; 00063 u_angle_min.real = this->angle_min; 00064 *(outbuffer + offset + 0) = (u_angle_min.base >> (8 * 0)) & 0xFF; 00065 *(outbuffer + offset + 1) = (u_angle_min.base >> (8 * 1)) & 0xFF; 00066 *(outbuffer + offset + 2) = (u_angle_min.base >> (8 * 2)) & 0xFF; 00067 *(outbuffer + offset + 3) = (u_angle_min.base >> (8 * 3)) & 0xFF; 00068 offset += sizeof(this->angle_min); 00069 union { 00070 float real; 00071 uint32_t base; 00072 } u_angle_max; 00073 u_angle_max.real = this->angle_max; 00074 *(outbuffer + offset + 0) = (u_angle_max.base >> (8 * 0)) & 0xFF; 00075 *(outbuffer + offset + 1) = (u_angle_max.base >> (8 * 1)) & 0xFF; 00076 *(outbuffer + offset + 2) = (u_angle_max.base >> (8 * 2)) & 0xFF; 00077 *(outbuffer + offset + 3) = (u_angle_max.base >> (8 * 3)) & 0xFF; 00078 offset += sizeof(this->angle_max); 00079 union { 00080 float real; 00081 uint32_t base; 00082 } u_angle_increment; 00083 u_angle_increment.real = this->angle_increment; 00084 *(outbuffer + offset + 0) = (u_angle_increment.base >> (8 * 0)) & 0xFF; 00085 *(outbuffer + offset + 1) = (u_angle_increment.base >> (8 * 1)) & 0xFF; 00086 *(outbuffer + offset + 2) = (u_angle_increment.base >> (8 * 2)) & 0xFF; 00087 *(outbuffer + offset + 3) = (u_angle_increment.base >> (8 * 3)) & 0xFF; 00088 offset += sizeof(this->angle_increment); 00089 union { 00090 float real; 00091 uint32_t base; 00092 } u_time_increment; 00093 u_time_increment.real = this->time_increment; 00094 *(outbuffer + offset + 0) = (u_time_increment.base >> (8 * 0)) & 0xFF; 00095 *(outbuffer + offset + 1) = (u_time_increment.base >> (8 * 1)) & 0xFF; 00096 *(outbuffer + offset + 2) = (u_time_increment.base >> (8 * 2)) & 0xFF; 00097 *(outbuffer + offset + 3) = (u_time_increment.base >> (8 * 3)) & 0xFF; 00098 offset += sizeof(this->time_increment); 00099 union { 00100 float real; 00101 uint32_t base; 00102 } u_scan_time; 00103 u_scan_time.real = this->scan_time; 00104 *(outbuffer + offset + 0) = (u_scan_time.base >> (8 * 0)) & 0xFF; 00105 *(outbuffer + offset + 1) = (u_scan_time.base >> (8 * 1)) & 0xFF; 00106 *(outbuffer + offset + 2) = (u_scan_time.base >> (8 * 2)) & 0xFF; 00107 *(outbuffer + offset + 3) = (u_scan_time.base >> (8 * 3)) & 0xFF; 00108 offset += sizeof(this->scan_time); 00109 union { 00110 float real; 00111 uint32_t base; 00112 } u_range_min; 00113 u_range_min.real = this->range_min; 00114 *(outbuffer + offset + 0) = (u_range_min.base >> (8 * 0)) & 0xFF; 00115 *(outbuffer + offset + 1) = (u_range_min.base >> (8 * 1)) & 0xFF; 00116 *(outbuffer + offset + 2) = (u_range_min.base >> (8 * 2)) & 0xFF; 00117 *(outbuffer + offset + 3) = (u_range_min.base >> (8 * 3)) & 0xFF; 00118 offset += sizeof(this->range_min); 00119 union { 00120 float real; 00121 uint32_t base; 00122 } u_range_max; 00123 u_range_max.real = this->range_max; 00124 *(outbuffer + offset + 0) = (u_range_max.base >> (8 * 0)) & 0xFF; 00125 *(outbuffer + offset + 1) = (u_range_max.base >> (8 * 1)) & 0xFF; 00126 *(outbuffer + offset + 2) = (u_range_max.base >> (8 * 2)) & 0xFF; 00127 *(outbuffer + offset + 3) = (u_range_max.base >> (8 * 3)) & 0xFF; 00128 offset += sizeof(this->range_max); 00129 *(outbuffer + offset + 0) = (this->ranges_length >> (8 * 0)) & 0xFF; 00130 *(outbuffer + offset + 1) = (this->ranges_length >> (8 * 1)) & 0xFF; 00131 *(outbuffer + offset + 2) = (this->ranges_length >> (8 * 2)) & 0xFF; 00132 *(outbuffer + offset + 3) = (this->ranges_length >> (8 * 3)) & 0xFF; 00133 offset += sizeof(this->ranges_length); 00134 for( uint32_t i = 0; i < ranges_length; i++){ 00135 union { 00136 float real; 00137 uint32_t base; 00138 } u_rangesi; 00139 u_rangesi.real = this->ranges[i]; 00140 *(outbuffer + offset + 0) = (u_rangesi.base >> (8 * 0)) & 0xFF; 00141 *(outbuffer + offset + 1) = (u_rangesi.base >> (8 * 1)) & 0xFF; 00142 *(outbuffer + offset + 2) = (u_rangesi.base >> (8 * 2)) & 0xFF; 00143 *(outbuffer + offset + 3) = (u_rangesi.base >> (8 * 3)) & 0xFF; 00144 offset += sizeof(this->ranges[i]); 00145 } 00146 *(outbuffer + offset + 0) = (this->intensities_length >> (8 * 0)) & 0xFF; 00147 *(outbuffer + offset + 1) = (this->intensities_length >> (8 * 1)) & 0xFF; 00148 *(outbuffer + offset + 2) = (this->intensities_length >> (8 * 2)) & 0xFF; 00149 *(outbuffer + offset + 3) = (this->intensities_length >> (8 * 3)) & 0xFF; 00150 offset += sizeof(this->intensities_length); 00151 for( uint32_t i = 0; i < intensities_length; i++){ 00152 union { 00153 float real; 00154 uint32_t base; 00155 } u_intensitiesi; 00156 u_intensitiesi.real = this->intensities[i]; 00157 *(outbuffer + offset + 0) = (u_intensitiesi.base >> (8 * 0)) & 0xFF; 00158 *(outbuffer + offset + 1) = (u_intensitiesi.base >> (8 * 1)) & 0xFF; 00159 *(outbuffer + offset + 2) = (u_intensitiesi.base >> (8 * 2)) & 0xFF; 00160 *(outbuffer + offset + 3) = (u_intensitiesi.base >> (8 * 3)) & 0xFF; 00161 offset += sizeof(this->intensities[i]); 00162 } 00163 return offset; 00164 } 00165 00166 virtual int deserialize(unsigned char *inbuffer) 00167 { 00168 int offset = 0; 00169 offset += this->header.deserialize(inbuffer + offset); 00170 union { 00171 float real; 00172 uint32_t base; 00173 } u_angle_min; 00174 u_angle_min.base = 0; 00175 u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00176 u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00177 u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00178 u_angle_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00179 this->angle_min = u_angle_min.real; 00180 offset += sizeof(this->angle_min); 00181 union { 00182 float real; 00183 uint32_t base; 00184 } u_angle_max; 00185 u_angle_max.base = 0; 00186 u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00187 u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00188 u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00189 u_angle_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00190 this->angle_max = u_angle_max.real; 00191 offset += sizeof(this->angle_max); 00192 union { 00193 float real; 00194 uint32_t base; 00195 } u_angle_increment; 00196 u_angle_increment.base = 0; 00197 u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00198 u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00199 u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00200 u_angle_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00201 this->angle_increment = u_angle_increment.real; 00202 offset += sizeof(this->angle_increment); 00203 union { 00204 float real; 00205 uint32_t base; 00206 } u_time_increment; 00207 u_time_increment.base = 0; 00208 u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00209 u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00210 u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00211 u_time_increment.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00212 this->time_increment = u_time_increment.real; 00213 offset += sizeof(this->time_increment); 00214 union { 00215 float real; 00216 uint32_t base; 00217 } u_scan_time; 00218 u_scan_time.base = 0; 00219 u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00220 u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00221 u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00222 u_scan_time.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00223 this->scan_time = u_scan_time.real; 00224 offset += sizeof(this->scan_time); 00225 union { 00226 float real; 00227 uint32_t base; 00228 } u_range_min; 00229 u_range_min.base = 0; 00230 u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00231 u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00232 u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00233 u_range_min.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00234 this->range_min = u_range_min.real; 00235 offset += sizeof(this->range_min); 00236 union { 00237 float real; 00238 uint32_t base; 00239 } u_range_max; 00240 u_range_max.base = 0; 00241 u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00242 u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00243 u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00244 u_range_max.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00245 this->range_max = u_range_max.real; 00246 offset += sizeof(this->range_max); 00247 uint32_t ranges_lengthT = ((uint32_t) (*(inbuffer + offset))); 00248 ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00249 ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00250 ranges_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00251 offset += sizeof(this->ranges_length); 00252 if(ranges_lengthT > ranges_length) 00253 this->ranges = (float*)realloc(this->ranges, ranges_lengthT * sizeof(float)); 00254 ranges_length = ranges_lengthT; 00255 for( uint32_t i = 0; i < ranges_length; i++){ 00256 union { 00257 float real; 00258 uint32_t base; 00259 } u_st_ranges; 00260 u_st_ranges.base = 0; 00261 u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00262 u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00263 u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00264 u_st_ranges.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00265 this->st_ranges = u_st_ranges.real; 00266 offset += sizeof(this->st_ranges); 00267 memcpy( &(this->ranges[i]), &(this->st_ranges), sizeof(float)); 00268 } 00269 uint32_t intensities_lengthT = ((uint32_t) (*(inbuffer + offset))); 00270 intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00271 intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00272 intensities_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00273 offset += sizeof(this->intensities_length); 00274 if(intensities_lengthT > intensities_length) 00275 this->intensities = (float*)realloc(this->intensities, intensities_lengthT * sizeof(float)); 00276 intensities_length = intensities_lengthT; 00277 for( uint32_t i = 0; i < intensities_length; i++){ 00278 union { 00279 float real; 00280 uint32_t base; 00281 } u_st_intensities; 00282 u_st_intensities.base = 0; 00283 u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00284 u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00285 u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00286 u_st_intensities.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00287 this->st_intensities = u_st_intensities.real; 00288 offset += sizeof(this->st_intensities); 00289 memcpy( &(this->intensities[i]), &(this->st_intensities), sizeof(float)); 00290 } 00291 return offset; 00292 } 00293 00294 const char * getType(){ return "sensor_msgs/LaserScan"; }; 00295 const char * getMD5(){ return "90c7ef2dc6895d81024acba2ac42f369"; }; 00296 00297 }; 00298 00299 } 00300 #endif
Generated on Tue Jul 12 2022 21:32:16 by 1.7.2