catchrobo2022
catchrobo2022 mbed LPC1768 メインプログラム
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
JointTrajectoryPointWithType.h
00001 #ifndef _ROS_jsk_interactive_marker_JointTrajectoryPointWithType_h 00002 #define _ROS_jsk_interactive_marker_JointTrajectoryPointWithType_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "ros/duration.h" 00009 00010 namespace jsk_interactive_marker 00011 { 00012 00013 class JointTrajectoryPointWithType : public ros::Msg 00014 { 00015 public: 00016 typedef int8_t _type_type; 00017 _type_type type; 00018 typedef const char* _args_type; 00019 _args_type args; 00020 typedef bool _wait_type; 00021 _wait_type wait; 00022 uint32_t positions_length; 00023 typedef double _positions_type; 00024 _positions_type st_positions; 00025 _positions_type * positions; 00026 uint32_t velocities_length; 00027 typedef double _velocities_type; 00028 _velocities_type st_velocities; 00029 _velocities_type * velocities; 00030 uint32_t accelerations_length; 00031 typedef double _accelerations_type; 00032 _accelerations_type st_accelerations; 00033 _accelerations_type * accelerations; 00034 uint32_t effort_length; 00035 typedef double _effort_type; 00036 _effort_type st_effort; 00037 _effort_type * effort; 00038 typedef ros::Duration _time_from_start_type; 00039 _time_from_start_type time_from_start; 00040 enum { JOINT_INTERPOLATION = 0 }; 00041 enum { LINEAR_INTERPOLATION = 1 }; 00042 enum { COLLISION_AVOIDANCE = 2 }; 00043 enum { CLOSE_HAND = 10 }; 00044 enum { OPEN_HAND = 11 }; 00045 00046 JointTrajectoryPointWithType(): 00047 type(0), 00048 args(""), 00049 wait(0), 00050 positions_length(0), positions(NULL), 00051 velocities_length(0), velocities(NULL), 00052 accelerations_length(0), accelerations(NULL), 00053 effort_length(0), effort(NULL), 00054 time_from_start() 00055 { 00056 } 00057 00058 virtual int serialize(unsigned char *outbuffer) const 00059 { 00060 int offset = 0; 00061 union { 00062 int8_t real; 00063 uint8_t base; 00064 } u_type; 00065 u_type.real = this->type; 00066 *(outbuffer + offset + 0) = (u_type.base >> (8 * 0)) & 0xFF; 00067 offset += sizeof(this->type); 00068 uint32_t length_args = strlen(this->args); 00069 varToArr(outbuffer + offset, length_args); 00070 offset += 4; 00071 memcpy(outbuffer + offset, this->args, length_args); 00072 offset += length_args; 00073 union { 00074 bool real; 00075 uint8_t base; 00076 } u_wait; 00077 u_wait.real = this->wait; 00078 *(outbuffer + offset + 0) = (u_wait.base >> (8 * 0)) & 0xFF; 00079 offset += sizeof(this->wait); 00080 *(outbuffer + offset + 0) = (this->positions_length >> (8 * 0)) & 0xFF; 00081 *(outbuffer + offset + 1) = (this->positions_length >> (8 * 1)) & 0xFF; 00082 *(outbuffer + offset + 2) = (this->positions_length >> (8 * 2)) & 0xFF; 00083 *(outbuffer + offset + 3) = (this->positions_length >> (8 * 3)) & 0xFF; 00084 offset += sizeof(this->positions_length); 00085 for( uint32_t i = 0; i < positions_length; i++){ 00086 union { 00087 double real; 00088 uint64_t base; 00089 } u_positionsi; 00090 u_positionsi.real = this->positions[i]; 00091 *(outbuffer + offset + 0) = (u_positionsi.base >> (8 * 0)) & 0xFF; 00092 *(outbuffer + offset + 1) = (u_positionsi.base >> (8 * 1)) & 0xFF; 00093 *(outbuffer + offset + 2) = (u_positionsi.base >> (8 * 2)) & 0xFF; 00094 *(outbuffer + offset + 3) = (u_positionsi.base >> (8 * 3)) & 0xFF; 00095 *(outbuffer + offset + 4) = (u_positionsi.base >> (8 * 4)) & 0xFF; 00096 *(outbuffer + offset + 5) = (u_positionsi.base >> (8 * 5)) & 0xFF; 00097 *(outbuffer + offset + 6) = (u_positionsi.base >> (8 * 6)) & 0xFF; 00098 *(outbuffer + offset + 7) = (u_positionsi.base >> (8 * 7)) & 0xFF; 00099 offset += sizeof(this->positions[i]); 00100 } 00101 *(outbuffer + offset + 0) = (this->velocities_length >> (8 * 0)) & 0xFF; 00102 *(outbuffer + offset + 1) = (this->velocities_length >> (8 * 1)) & 0xFF; 00103 *(outbuffer + offset + 2) = (this->velocities_length >> (8 * 2)) & 0xFF; 00104 *(outbuffer + offset + 3) = (this->velocities_length >> (8 * 3)) & 0xFF; 00105 offset += sizeof(this->velocities_length); 00106 for( uint32_t i = 0; i < velocities_length; i++){ 00107 union { 00108 double real; 00109 uint64_t base; 00110 } u_velocitiesi; 00111 u_velocitiesi.real = this->velocities[i]; 00112 *(outbuffer + offset + 0) = (u_velocitiesi.base >> (8 * 0)) & 0xFF; 00113 *(outbuffer + offset + 1) = (u_velocitiesi.base >> (8 * 1)) & 0xFF; 00114 *(outbuffer + offset + 2) = (u_velocitiesi.base >> (8 * 2)) & 0xFF; 00115 *(outbuffer + offset + 3) = (u_velocitiesi.base >> (8 * 3)) & 0xFF; 00116 *(outbuffer + offset + 4) = (u_velocitiesi.base >> (8 * 4)) & 0xFF; 00117 *(outbuffer + offset + 5) = (u_velocitiesi.base >> (8 * 5)) & 0xFF; 00118 *(outbuffer + offset + 6) = (u_velocitiesi.base >> (8 * 6)) & 0xFF; 00119 *(outbuffer + offset + 7) = (u_velocitiesi.base >> (8 * 7)) & 0xFF; 00120 offset += sizeof(this->velocities[i]); 00121 } 00122 *(outbuffer + offset + 0) = (this->accelerations_length >> (8 * 0)) & 0xFF; 00123 *(outbuffer + offset + 1) = (this->accelerations_length >> (8 * 1)) & 0xFF; 00124 *(outbuffer + offset + 2) = (this->accelerations_length >> (8 * 2)) & 0xFF; 00125 *(outbuffer + offset + 3) = (this->accelerations_length >> (8 * 3)) & 0xFF; 00126 offset += sizeof(this->accelerations_length); 00127 for( uint32_t i = 0; i < accelerations_length; i++){ 00128 union { 00129 double real; 00130 uint64_t base; 00131 } u_accelerationsi; 00132 u_accelerationsi.real = this->accelerations[i]; 00133 *(outbuffer + offset + 0) = (u_accelerationsi.base >> (8 * 0)) & 0xFF; 00134 *(outbuffer + offset + 1) = (u_accelerationsi.base >> (8 * 1)) & 0xFF; 00135 *(outbuffer + offset + 2) = (u_accelerationsi.base >> (8 * 2)) & 0xFF; 00136 *(outbuffer + offset + 3) = (u_accelerationsi.base >> (8 * 3)) & 0xFF; 00137 *(outbuffer + offset + 4) = (u_accelerationsi.base >> (8 * 4)) & 0xFF; 00138 *(outbuffer + offset + 5) = (u_accelerationsi.base >> (8 * 5)) & 0xFF; 00139 *(outbuffer + offset + 6) = (u_accelerationsi.base >> (8 * 6)) & 0xFF; 00140 *(outbuffer + offset + 7) = (u_accelerationsi.base >> (8 * 7)) & 0xFF; 00141 offset += sizeof(this->accelerations[i]); 00142 } 00143 *(outbuffer + offset + 0) = (this->effort_length >> (8 * 0)) & 0xFF; 00144 *(outbuffer + offset + 1) = (this->effort_length >> (8 * 1)) & 0xFF; 00145 *(outbuffer + offset + 2) = (this->effort_length >> (8 * 2)) & 0xFF; 00146 *(outbuffer + offset + 3) = (this->effort_length >> (8 * 3)) & 0xFF; 00147 offset += sizeof(this->effort_length); 00148 for( uint32_t i = 0; i < effort_length; i++){ 00149 union { 00150 double real; 00151 uint64_t base; 00152 } u_efforti; 00153 u_efforti.real = this->effort[i]; 00154 *(outbuffer + offset + 0) = (u_efforti.base >> (8 * 0)) & 0xFF; 00155 *(outbuffer + offset + 1) = (u_efforti.base >> (8 * 1)) & 0xFF; 00156 *(outbuffer + offset + 2) = (u_efforti.base >> (8 * 2)) & 0xFF; 00157 *(outbuffer + offset + 3) = (u_efforti.base >> (8 * 3)) & 0xFF; 00158 *(outbuffer + offset + 4) = (u_efforti.base >> (8 * 4)) & 0xFF; 00159 *(outbuffer + offset + 5) = (u_efforti.base >> (8 * 5)) & 0xFF; 00160 *(outbuffer + offset + 6) = (u_efforti.base >> (8 * 6)) & 0xFF; 00161 *(outbuffer + offset + 7) = (u_efforti.base >> (8 * 7)) & 0xFF; 00162 offset += sizeof(this->effort[i]); 00163 } 00164 *(outbuffer + offset + 0) = (this->time_from_start.sec >> (8 * 0)) & 0xFF; 00165 *(outbuffer + offset + 1) = (this->time_from_start.sec >> (8 * 1)) & 0xFF; 00166 *(outbuffer + offset + 2) = (this->time_from_start.sec >> (8 * 2)) & 0xFF; 00167 *(outbuffer + offset + 3) = (this->time_from_start.sec >> (8 * 3)) & 0xFF; 00168 offset += sizeof(this->time_from_start.sec); 00169 *(outbuffer + offset + 0) = (this->time_from_start.nsec >> (8 * 0)) & 0xFF; 00170 *(outbuffer + offset + 1) = (this->time_from_start.nsec >> (8 * 1)) & 0xFF; 00171 *(outbuffer + offset + 2) = (this->time_from_start.nsec >> (8 * 2)) & 0xFF; 00172 *(outbuffer + offset + 3) = (this->time_from_start.nsec >> (8 * 3)) & 0xFF; 00173 offset += sizeof(this->time_from_start.nsec); 00174 return offset; 00175 } 00176 00177 virtual int deserialize(unsigned char *inbuffer) 00178 { 00179 int offset = 0; 00180 union { 00181 int8_t real; 00182 uint8_t base; 00183 } u_type; 00184 u_type.base = 0; 00185 u_type.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00186 this->type = u_type.real; 00187 offset += sizeof(this->type); 00188 uint32_t length_args; 00189 arrToVar(length_args, (inbuffer + offset)); 00190 offset += 4; 00191 for(unsigned int k= offset; k< offset+length_args; ++k){ 00192 inbuffer[k-1]=inbuffer[k]; 00193 } 00194 inbuffer[offset+length_args-1]=0; 00195 this->args = (char *)(inbuffer + offset-1); 00196 offset += length_args; 00197 union { 00198 bool real; 00199 uint8_t base; 00200 } u_wait; 00201 u_wait.base = 0; 00202 u_wait.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00203 this->wait = u_wait.real; 00204 offset += sizeof(this->wait); 00205 uint32_t positions_lengthT = ((uint32_t) (*(inbuffer + offset))); 00206 positions_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00207 positions_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00208 positions_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00209 offset += sizeof(this->positions_length); 00210 if(positions_lengthT > positions_length) 00211 this->positions = (double*)realloc(this->positions, positions_lengthT * sizeof(double)); 00212 positions_length = positions_lengthT; 00213 for( uint32_t i = 0; i < positions_length; i++){ 00214 union { 00215 double real; 00216 uint64_t base; 00217 } u_st_positions; 00218 u_st_positions.base = 0; 00219 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00220 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00221 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00222 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00223 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00224 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00225 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00226 u_st_positions.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00227 this->st_positions = u_st_positions.real; 00228 offset += sizeof(this->st_positions); 00229 memcpy( &(this->positions[i]), &(this->st_positions), sizeof(double)); 00230 } 00231 uint32_t velocities_lengthT = ((uint32_t) (*(inbuffer + offset))); 00232 velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00233 velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00234 velocities_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00235 offset += sizeof(this->velocities_length); 00236 if(velocities_lengthT > velocities_length) 00237 this->velocities = (double*)realloc(this->velocities, velocities_lengthT * sizeof(double)); 00238 velocities_length = velocities_lengthT; 00239 for( uint32_t i = 0; i < velocities_length; i++){ 00240 union { 00241 double real; 00242 uint64_t base; 00243 } u_st_velocities; 00244 u_st_velocities.base = 0; 00245 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00246 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00247 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00248 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00249 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00250 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00251 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00252 u_st_velocities.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00253 this->st_velocities = u_st_velocities.real; 00254 offset += sizeof(this->st_velocities); 00255 memcpy( &(this->velocities[i]), &(this->st_velocities), sizeof(double)); 00256 } 00257 uint32_t accelerations_lengthT = ((uint32_t) (*(inbuffer + offset))); 00258 accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00259 accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00260 accelerations_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00261 offset += sizeof(this->accelerations_length); 00262 if(accelerations_lengthT > accelerations_length) 00263 this->accelerations = (double*)realloc(this->accelerations, accelerations_lengthT * sizeof(double)); 00264 accelerations_length = accelerations_lengthT; 00265 for( uint32_t i = 0; i < accelerations_length; i++){ 00266 union { 00267 double real; 00268 uint64_t base; 00269 } u_st_accelerations; 00270 u_st_accelerations.base = 0; 00271 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00272 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00273 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00274 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00275 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00276 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00277 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00278 u_st_accelerations.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00279 this->st_accelerations = u_st_accelerations.real; 00280 offset += sizeof(this->st_accelerations); 00281 memcpy( &(this->accelerations[i]), &(this->st_accelerations), sizeof(double)); 00282 } 00283 uint32_t effort_lengthT = ((uint32_t) (*(inbuffer + offset))); 00284 effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00285 effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00286 effort_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00287 offset += sizeof(this->effort_length); 00288 if(effort_lengthT > effort_length) 00289 this->effort = (double*)realloc(this->effort, effort_lengthT * sizeof(double)); 00290 effort_length = effort_lengthT; 00291 for( uint32_t i = 0; i < effort_length; i++){ 00292 union { 00293 double real; 00294 uint64_t base; 00295 } u_st_effort; 00296 u_st_effort.base = 0; 00297 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00298 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00299 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00300 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00301 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00302 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00303 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00304 u_st_effort.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00305 this->st_effort = u_st_effort.real; 00306 offset += sizeof(this->st_effort); 00307 memcpy( &(this->effort[i]), &(this->st_effort), sizeof(double)); 00308 } 00309 this->time_from_start.sec = ((uint32_t) (*(inbuffer + offset))); 00310 this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00311 this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00312 this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00313 offset += sizeof(this->time_from_start.sec); 00314 this->time_from_start.nsec = ((uint32_t) (*(inbuffer + offset))); 00315 this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00316 this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00317 this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00318 offset += sizeof(this->time_from_start.nsec); 00319 return offset; 00320 } 00321 00322 virtual const char * getType(){ return "jsk_interactive_marker/JointTrajectoryPointWithType"; }; 00323 virtual const char * getMD5(){ return "990bd6a1d9e03cc634e576f569783816"; }; 00324 00325 }; 00326 00327 } 00328 #endif
Generated on Mon Sep 26 2022 13:47:01 by
1.7.2