Irfan Tito Kurniawan
/
ros_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
PolynomialSegment4D.h
00001 #ifndef _ROS_mav_planning_msgs_PolynomialSegment4D_h 00002 #define _ROS_mav_planning_msgs_PolynomialSegment4D_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 "ros/duration.h" 00010 00011 namespace mav_planning_msgs 00012 { 00013 00014 class PolynomialSegment4D : public ros::Msg 00015 { 00016 public: 00017 typedef std_msgs::Header _header_type; 00018 _header_type header; 00019 typedef int32_t _num_coeffs_type; 00020 _num_coeffs_type num_coeffs; 00021 typedef ros::Duration _segment_time_type; 00022 _segment_time_type segment_time; 00023 uint32_t x_length; 00024 typedef double _x_type; 00025 _x_type st_x; 00026 _x_type * x; 00027 uint32_t y_length; 00028 typedef double _y_type; 00029 _y_type st_y; 00030 _y_type * y; 00031 uint32_t z_length; 00032 typedef double _z_type; 00033 _z_type st_z; 00034 _z_type * z; 00035 uint32_t yaw_length; 00036 typedef double _yaw_type; 00037 _yaw_type st_yaw; 00038 _yaw_type * yaw; 00039 00040 PolynomialSegment4D(): 00041 header(), 00042 num_coeffs(0), 00043 segment_time(), 00044 x_length(0), x(NULL), 00045 y_length(0), y(NULL), 00046 z_length(0), z(NULL), 00047 yaw_length(0), yaw(NULL) 00048 { 00049 } 00050 00051 virtual int serialize(unsigned char *outbuffer) const 00052 { 00053 int offset = 0; 00054 offset += this->header.serialize(outbuffer + offset); 00055 union { 00056 int32_t real; 00057 uint32_t base; 00058 } u_num_coeffs; 00059 u_num_coeffs.real = this->num_coeffs; 00060 *(outbuffer + offset + 0) = (u_num_coeffs.base >> (8 * 0)) & 0xFF; 00061 *(outbuffer + offset + 1) = (u_num_coeffs.base >> (8 * 1)) & 0xFF; 00062 *(outbuffer + offset + 2) = (u_num_coeffs.base >> (8 * 2)) & 0xFF; 00063 *(outbuffer + offset + 3) = (u_num_coeffs.base >> (8 * 3)) & 0xFF; 00064 offset += sizeof(this->num_coeffs); 00065 *(outbuffer + offset + 0) = (this->segment_time.sec >> (8 * 0)) & 0xFF; 00066 *(outbuffer + offset + 1) = (this->segment_time.sec >> (8 * 1)) & 0xFF; 00067 *(outbuffer + offset + 2) = (this->segment_time.sec >> (8 * 2)) & 0xFF; 00068 *(outbuffer + offset + 3) = (this->segment_time.sec >> (8 * 3)) & 0xFF; 00069 offset += sizeof(this->segment_time.sec); 00070 *(outbuffer + offset + 0) = (this->segment_time.nsec >> (8 * 0)) & 0xFF; 00071 *(outbuffer + offset + 1) = (this->segment_time.nsec >> (8 * 1)) & 0xFF; 00072 *(outbuffer + offset + 2) = (this->segment_time.nsec >> (8 * 2)) & 0xFF; 00073 *(outbuffer + offset + 3) = (this->segment_time.nsec >> (8 * 3)) & 0xFF; 00074 offset += sizeof(this->segment_time.nsec); 00075 *(outbuffer + offset + 0) = (this->x_length >> (8 * 0)) & 0xFF; 00076 *(outbuffer + offset + 1) = (this->x_length >> (8 * 1)) & 0xFF; 00077 *(outbuffer + offset + 2) = (this->x_length >> (8 * 2)) & 0xFF; 00078 *(outbuffer + offset + 3) = (this->x_length >> (8 * 3)) & 0xFF; 00079 offset += sizeof(this->x_length); 00080 for( uint32_t i = 0; i < x_length; i++){ 00081 union { 00082 double real; 00083 uint64_t base; 00084 } u_xi; 00085 u_xi.real = this->x[i]; 00086 *(outbuffer + offset + 0) = (u_xi.base >> (8 * 0)) & 0xFF; 00087 *(outbuffer + offset + 1) = (u_xi.base >> (8 * 1)) & 0xFF; 00088 *(outbuffer + offset + 2) = (u_xi.base >> (8 * 2)) & 0xFF; 00089 *(outbuffer + offset + 3) = (u_xi.base >> (8 * 3)) & 0xFF; 00090 *(outbuffer + offset + 4) = (u_xi.base >> (8 * 4)) & 0xFF; 00091 *(outbuffer + offset + 5) = (u_xi.base >> (8 * 5)) & 0xFF; 00092 *(outbuffer + offset + 6) = (u_xi.base >> (8 * 6)) & 0xFF; 00093 *(outbuffer + offset + 7) = (u_xi.base >> (8 * 7)) & 0xFF; 00094 offset += sizeof(this->x[i]); 00095 } 00096 *(outbuffer + offset + 0) = (this->y_length >> (8 * 0)) & 0xFF; 00097 *(outbuffer + offset + 1) = (this->y_length >> (8 * 1)) & 0xFF; 00098 *(outbuffer + offset + 2) = (this->y_length >> (8 * 2)) & 0xFF; 00099 *(outbuffer + offset + 3) = (this->y_length >> (8 * 3)) & 0xFF; 00100 offset += sizeof(this->y_length); 00101 for( uint32_t i = 0; i < y_length; i++){ 00102 union { 00103 double real; 00104 uint64_t base; 00105 } u_yi; 00106 u_yi.real = this->y[i]; 00107 *(outbuffer + offset + 0) = (u_yi.base >> (8 * 0)) & 0xFF; 00108 *(outbuffer + offset + 1) = (u_yi.base >> (8 * 1)) & 0xFF; 00109 *(outbuffer + offset + 2) = (u_yi.base >> (8 * 2)) & 0xFF; 00110 *(outbuffer + offset + 3) = (u_yi.base >> (8 * 3)) & 0xFF; 00111 *(outbuffer + offset + 4) = (u_yi.base >> (8 * 4)) & 0xFF; 00112 *(outbuffer + offset + 5) = (u_yi.base >> (8 * 5)) & 0xFF; 00113 *(outbuffer + offset + 6) = (u_yi.base >> (8 * 6)) & 0xFF; 00114 *(outbuffer + offset + 7) = (u_yi.base >> (8 * 7)) & 0xFF; 00115 offset += sizeof(this->y[i]); 00116 } 00117 *(outbuffer + offset + 0) = (this->z_length >> (8 * 0)) & 0xFF; 00118 *(outbuffer + offset + 1) = (this->z_length >> (8 * 1)) & 0xFF; 00119 *(outbuffer + offset + 2) = (this->z_length >> (8 * 2)) & 0xFF; 00120 *(outbuffer + offset + 3) = (this->z_length >> (8 * 3)) & 0xFF; 00121 offset += sizeof(this->z_length); 00122 for( uint32_t i = 0; i < z_length; i++){ 00123 union { 00124 double real; 00125 uint64_t base; 00126 } u_zi; 00127 u_zi.real = this->z[i]; 00128 *(outbuffer + offset + 0) = (u_zi.base >> (8 * 0)) & 0xFF; 00129 *(outbuffer + offset + 1) = (u_zi.base >> (8 * 1)) & 0xFF; 00130 *(outbuffer + offset + 2) = (u_zi.base >> (8 * 2)) & 0xFF; 00131 *(outbuffer + offset + 3) = (u_zi.base >> (8 * 3)) & 0xFF; 00132 *(outbuffer + offset + 4) = (u_zi.base >> (8 * 4)) & 0xFF; 00133 *(outbuffer + offset + 5) = (u_zi.base >> (8 * 5)) & 0xFF; 00134 *(outbuffer + offset + 6) = (u_zi.base >> (8 * 6)) & 0xFF; 00135 *(outbuffer + offset + 7) = (u_zi.base >> (8 * 7)) & 0xFF; 00136 offset += sizeof(this->z[i]); 00137 } 00138 *(outbuffer + offset + 0) = (this->yaw_length >> (8 * 0)) & 0xFF; 00139 *(outbuffer + offset + 1) = (this->yaw_length >> (8 * 1)) & 0xFF; 00140 *(outbuffer + offset + 2) = (this->yaw_length >> (8 * 2)) & 0xFF; 00141 *(outbuffer + offset + 3) = (this->yaw_length >> (8 * 3)) & 0xFF; 00142 offset += sizeof(this->yaw_length); 00143 for( uint32_t i = 0; i < yaw_length; i++){ 00144 union { 00145 double real; 00146 uint64_t base; 00147 } u_yawi; 00148 u_yawi.real = this->yaw[i]; 00149 *(outbuffer + offset + 0) = (u_yawi.base >> (8 * 0)) & 0xFF; 00150 *(outbuffer + offset + 1) = (u_yawi.base >> (8 * 1)) & 0xFF; 00151 *(outbuffer + offset + 2) = (u_yawi.base >> (8 * 2)) & 0xFF; 00152 *(outbuffer + offset + 3) = (u_yawi.base >> (8 * 3)) & 0xFF; 00153 *(outbuffer + offset + 4) = (u_yawi.base >> (8 * 4)) & 0xFF; 00154 *(outbuffer + offset + 5) = (u_yawi.base >> (8 * 5)) & 0xFF; 00155 *(outbuffer + offset + 6) = (u_yawi.base >> (8 * 6)) & 0xFF; 00156 *(outbuffer + offset + 7) = (u_yawi.base >> (8 * 7)) & 0xFF; 00157 offset += sizeof(this->yaw[i]); 00158 } 00159 return offset; 00160 } 00161 00162 virtual int deserialize(unsigned char *inbuffer) 00163 { 00164 int offset = 0; 00165 offset += this->header.deserialize(inbuffer + offset); 00166 union { 00167 int32_t real; 00168 uint32_t base; 00169 } u_num_coeffs; 00170 u_num_coeffs.base = 0; 00171 u_num_coeffs.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00172 u_num_coeffs.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00173 u_num_coeffs.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00174 u_num_coeffs.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00175 this->num_coeffs = u_num_coeffs.real; 00176 offset += sizeof(this->num_coeffs); 00177 this->segment_time.sec = ((uint32_t) (*(inbuffer + offset))); 00178 this->segment_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00179 this->segment_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00180 this->segment_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00181 offset += sizeof(this->segment_time.sec); 00182 this->segment_time.nsec = ((uint32_t) (*(inbuffer + offset))); 00183 this->segment_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00184 this->segment_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00185 this->segment_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00186 offset += sizeof(this->segment_time.nsec); 00187 uint32_t x_lengthT = ((uint32_t) (*(inbuffer + offset))); 00188 x_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00189 x_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00190 x_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00191 offset += sizeof(this->x_length); 00192 if(x_lengthT > x_length) 00193 this->x = (double*)realloc(this->x, x_lengthT * sizeof(double)); 00194 x_length = x_lengthT; 00195 for( uint32_t i = 0; i < x_length; i++){ 00196 union { 00197 double real; 00198 uint64_t base; 00199 } u_st_x; 00200 u_st_x.base = 0; 00201 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00202 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00203 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00204 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00205 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00206 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00207 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00208 u_st_x.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00209 this->st_x = u_st_x.real; 00210 offset += sizeof(this->st_x); 00211 memcpy( &(this->x[i]), &(this->st_x), sizeof(double)); 00212 } 00213 uint32_t y_lengthT = ((uint32_t) (*(inbuffer + offset))); 00214 y_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00215 y_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00216 y_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00217 offset += sizeof(this->y_length); 00218 if(y_lengthT > y_length) 00219 this->y = (double*)realloc(this->y, y_lengthT * sizeof(double)); 00220 y_length = y_lengthT; 00221 for( uint32_t i = 0; i < y_length; i++){ 00222 union { 00223 double real; 00224 uint64_t base; 00225 } u_st_y; 00226 u_st_y.base = 0; 00227 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00228 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00229 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00230 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00231 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00232 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00233 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00234 u_st_y.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00235 this->st_y = u_st_y.real; 00236 offset += sizeof(this->st_y); 00237 memcpy( &(this->y[i]), &(this->st_y), sizeof(double)); 00238 } 00239 uint32_t z_lengthT = ((uint32_t) (*(inbuffer + offset))); 00240 z_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00241 z_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00242 z_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00243 offset += sizeof(this->z_length); 00244 if(z_lengthT > z_length) 00245 this->z = (double*)realloc(this->z, z_lengthT * sizeof(double)); 00246 z_length = z_lengthT; 00247 for( uint32_t i = 0; i < z_length; i++){ 00248 union { 00249 double real; 00250 uint64_t base; 00251 } u_st_z; 00252 u_st_z.base = 0; 00253 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00254 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00255 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00256 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00257 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00258 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00259 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00260 u_st_z.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00261 this->st_z = u_st_z.real; 00262 offset += sizeof(this->st_z); 00263 memcpy( &(this->z[i]), &(this->st_z), sizeof(double)); 00264 } 00265 uint32_t yaw_lengthT = ((uint32_t) (*(inbuffer + offset))); 00266 yaw_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00267 yaw_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00268 yaw_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00269 offset += sizeof(this->yaw_length); 00270 if(yaw_lengthT > yaw_length) 00271 this->yaw = (double*)realloc(this->yaw, yaw_lengthT * sizeof(double)); 00272 yaw_length = yaw_lengthT; 00273 for( uint32_t i = 0; i < yaw_length; i++){ 00274 union { 00275 double real; 00276 uint64_t base; 00277 } u_st_yaw; 00278 u_st_yaw.base = 0; 00279 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00280 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00281 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00282 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00283 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00284 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00285 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00286 u_st_yaw.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00287 this->st_yaw = u_st_yaw.real; 00288 offset += sizeof(this->st_yaw); 00289 memcpy( &(this->yaw[i]), &(this->st_yaw), sizeof(double)); 00290 } 00291 return offset; 00292 } 00293 00294 const char * getType(){ return "mav_planning_msgs/PolynomialSegment4D"; }; 00295 const char * getMD5(){ return "c85fa40c94ff35d242df13d4d3a57809"; }; 00296 00297 }; 00298 00299 } 00300 #endif
Generated on Thu Jul 14 2022 10:09:24 by
1.7.2