catchrobo2022
/
catchrobo2022_mbed
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.
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
PositionIKRequest.h
00001 #ifndef _ROS_moveit_msgs_PositionIKRequest_h 00002 #define _ROS_moveit_msgs_PositionIKRequest_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "moveit_msgs/RobotState.h" 00009 #include "moveit_msgs/Constraints.h" 00010 #include "geometry_msgs/PoseStamped.h" 00011 #include "ros/duration.h" 00012 00013 namespace moveit_msgs 00014 { 00015 00016 class PositionIKRequest : public ros::Msg 00017 { 00018 public: 00019 typedef const char* _group_name_type; 00020 _group_name_type group_name; 00021 typedef moveit_msgs::RobotState _robot_state_type; 00022 _robot_state_type robot_state; 00023 typedef moveit_msgs::Constraints _constraints_type; 00024 _constraints_type constraints; 00025 typedef bool _avoid_collisions_type; 00026 _avoid_collisions_type avoid_collisions; 00027 typedef const char* _ik_link_name_type; 00028 _ik_link_name_type ik_link_name; 00029 typedef geometry_msgs::PoseStamped _pose_stamped_type; 00030 _pose_stamped_type pose_stamped; 00031 uint32_t ik_link_names_length; 00032 typedef char* _ik_link_names_type; 00033 _ik_link_names_type st_ik_link_names; 00034 _ik_link_names_type * ik_link_names; 00035 uint32_t pose_stamped_vector_length; 00036 typedef geometry_msgs::PoseStamped _pose_stamped_vector_type; 00037 _pose_stamped_vector_type st_pose_stamped_vector; 00038 _pose_stamped_vector_type * pose_stamped_vector; 00039 typedef ros::Duration _timeout_type; 00040 _timeout_type timeout; 00041 typedef int32_t _attempts_type; 00042 _attempts_type attempts; 00043 00044 PositionIKRequest(): 00045 group_name(""), 00046 robot_state(), 00047 constraints(), 00048 avoid_collisions(0), 00049 ik_link_name(""), 00050 pose_stamped(), 00051 ik_link_names_length(0), ik_link_names(NULL), 00052 pose_stamped_vector_length(0), pose_stamped_vector(NULL), 00053 timeout(), 00054 attempts(0) 00055 { 00056 } 00057 00058 virtual int serialize(unsigned char *outbuffer) const 00059 { 00060 int offset = 0; 00061 uint32_t length_group_name = strlen(this->group_name); 00062 varToArr(outbuffer + offset, length_group_name); 00063 offset += 4; 00064 memcpy(outbuffer + offset, this->group_name, length_group_name); 00065 offset += length_group_name; 00066 offset += this->robot_state.serialize(outbuffer + offset); 00067 offset += this->constraints.serialize(outbuffer + offset); 00068 union { 00069 bool real; 00070 uint8_t base; 00071 } u_avoid_collisions; 00072 u_avoid_collisions.real = this->avoid_collisions; 00073 *(outbuffer + offset + 0) = (u_avoid_collisions.base >> (8 * 0)) & 0xFF; 00074 offset += sizeof(this->avoid_collisions); 00075 uint32_t length_ik_link_name = strlen(this->ik_link_name); 00076 varToArr(outbuffer + offset, length_ik_link_name); 00077 offset += 4; 00078 memcpy(outbuffer + offset, this->ik_link_name, length_ik_link_name); 00079 offset += length_ik_link_name; 00080 offset += this->pose_stamped.serialize(outbuffer + offset); 00081 *(outbuffer + offset + 0) = (this->ik_link_names_length >> (8 * 0)) & 0xFF; 00082 *(outbuffer + offset + 1) = (this->ik_link_names_length >> (8 * 1)) & 0xFF; 00083 *(outbuffer + offset + 2) = (this->ik_link_names_length >> (8 * 2)) & 0xFF; 00084 *(outbuffer + offset + 3) = (this->ik_link_names_length >> (8 * 3)) & 0xFF; 00085 offset += sizeof(this->ik_link_names_length); 00086 for( uint32_t i = 0; i < ik_link_names_length; i++){ 00087 uint32_t length_ik_link_namesi = strlen(this->ik_link_names[i]); 00088 varToArr(outbuffer + offset, length_ik_link_namesi); 00089 offset += 4; 00090 memcpy(outbuffer + offset, this->ik_link_names[i], length_ik_link_namesi); 00091 offset += length_ik_link_namesi; 00092 } 00093 *(outbuffer + offset + 0) = (this->pose_stamped_vector_length >> (8 * 0)) & 0xFF; 00094 *(outbuffer + offset + 1) = (this->pose_stamped_vector_length >> (8 * 1)) & 0xFF; 00095 *(outbuffer + offset + 2) = (this->pose_stamped_vector_length >> (8 * 2)) & 0xFF; 00096 *(outbuffer + offset + 3) = (this->pose_stamped_vector_length >> (8 * 3)) & 0xFF; 00097 offset += sizeof(this->pose_stamped_vector_length); 00098 for( uint32_t i = 0; i < pose_stamped_vector_length; i++){ 00099 offset += this->pose_stamped_vector[i].serialize(outbuffer + offset); 00100 } 00101 *(outbuffer + offset + 0) = (this->timeout.sec >> (8 * 0)) & 0xFF; 00102 *(outbuffer + offset + 1) = (this->timeout.sec >> (8 * 1)) & 0xFF; 00103 *(outbuffer + offset + 2) = (this->timeout.sec >> (8 * 2)) & 0xFF; 00104 *(outbuffer + offset + 3) = (this->timeout.sec >> (8 * 3)) & 0xFF; 00105 offset += sizeof(this->timeout.sec); 00106 *(outbuffer + offset + 0) = (this->timeout.nsec >> (8 * 0)) & 0xFF; 00107 *(outbuffer + offset + 1) = (this->timeout.nsec >> (8 * 1)) & 0xFF; 00108 *(outbuffer + offset + 2) = (this->timeout.nsec >> (8 * 2)) & 0xFF; 00109 *(outbuffer + offset + 3) = (this->timeout.nsec >> (8 * 3)) & 0xFF; 00110 offset += sizeof(this->timeout.nsec); 00111 union { 00112 int32_t real; 00113 uint32_t base; 00114 } u_attempts; 00115 u_attempts.real = this->attempts; 00116 *(outbuffer + offset + 0) = (u_attempts.base >> (8 * 0)) & 0xFF; 00117 *(outbuffer + offset + 1) = (u_attempts.base >> (8 * 1)) & 0xFF; 00118 *(outbuffer + offset + 2) = (u_attempts.base >> (8 * 2)) & 0xFF; 00119 *(outbuffer + offset + 3) = (u_attempts.base >> (8 * 3)) & 0xFF; 00120 offset += sizeof(this->attempts); 00121 return offset; 00122 } 00123 00124 virtual int deserialize(unsigned char *inbuffer) 00125 { 00126 int offset = 0; 00127 uint32_t length_group_name; 00128 arrToVar(length_group_name, (inbuffer + offset)); 00129 offset += 4; 00130 for(unsigned int k= offset; k< offset+length_group_name; ++k){ 00131 inbuffer[k-1]=inbuffer[k]; 00132 } 00133 inbuffer[offset+length_group_name-1]=0; 00134 this->group_name = (char *)(inbuffer + offset-1); 00135 offset += length_group_name; 00136 offset += this->robot_state.deserialize(inbuffer + offset); 00137 offset += this->constraints.deserialize(inbuffer + offset); 00138 union { 00139 bool real; 00140 uint8_t base; 00141 } u_avoid_collisions; 00142 u_avoid_collisions.base = 0; 00143 u_avoid_collisions.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00144 this->avoid_collisions = u_avoid_collisions.real; 00145 offset += sizeof(this->avoid_collisions); 00146 uint32_t length_ik_link_name; 00147 arrToVar(length_ik_link_name, (inbuffer + offset)); 00148 offset += 4; 00149 for(unsigned int k= offset; k< offset+length_ik_link_name; ++k){ 00150 inbuffer[k-1]=inbuffer[k]; 00151 } 00152 inbuffer[offset+length_ik_link_name-1]=0; 00153 this->ik_link_name = (char *)(inbuffer + offset-1); 00154 offset += length_ik_link_name; 00155 offset += this->pose_stamped.deserialize(inbuffer + offset); 00156 uint32_t ik_link_names_lengthT = ((uint32_t) (*(inbuffer + offset))); 00157 ik_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00158 ik_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00159 ik_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00160 offset += sizeof(this->ik_link_names_length); 00161 if(ik_link_names_lengthT > ik_link_names_length) 00162 this->ik_link_names = (char**)realloc(this->ik_link_names, ik_link_names_lengthT * sizeof(char*)); 00163 ik_link_names_length = ik_link_names_lengthT; 00164 for( uint32_t i = 0; i < ik_link_names_length; i++){ 00165 uint32_t length_st_ik_link_names; 00166 arrToVar(length_st_ik_link_names, (inbuffer + offset)); 00167 offset += 4; 00168 for(unsigned int k= offset; k< offset+length_st_ik_link_names; ++k){ 00169 inbuffer[k-1]=inbuffer[k]; 00170 } 00171 inbuffer[offset+length_st_ik_link_names-1]=0; 00172 this->st_ik_link_names = (char *)(inbuffer + offset-1); 00173 offset += length_st_ik_link_names; 00174 memcpy( &(this->ik_link_names[i]), &(this->st_ik_link_names), sizeof(char*)); 00175 } 00176 uint32_t pose_stamped_vector_lengthT = ((uint32_t) (*(inbuffer + offset))); 00177 pose_stamped_vector_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00178 pose_stamped_vector_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00179 pose_stamped_vector_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00180 offset += sizeof(this->pose_stamped_vector_length); 00181 if(pose_stamped_vector_lengthT > pose_stamped_vector_length) 00182 this->pose_stamped_vector = (geometry_msgs::PoseStamped*)realloc(this->pose_stamped_vector, pose_stamped_vector_lengthT * sizeof(geometry_msgs::PoseStamped)); 00183 pose_stamped_vector_length = pose_stamped_vector_lengthT; 00184 for( uint32_t i = 0; i < pose_stamped_vector_length; i++){ 00185 offset += this->st_pose_stamped_vector.deserialize(inbuffer + offset); 00186 memcpy( &(this->pose_stamped_vector[i]), &(this->st_pose_stamped_vector), sizeof(geometry_msgs::PoseStamped)); 00187 } 00188 this->timeout.sec = ((uint32_t) (*(inbuffer + offset))); 00189 this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00190 this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00191 this->timeout.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00192 offset += sizeof(this->timeout.sec); 00193 this->timeout.nsec = ((uint32_t) (*(inbuffer + offset))); 00194 this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00195 this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00196 this->timeout.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00197 offset += sizeof(this->timeout.nsec); 00198 union { 00199 int32_t real; 00200 uint32_t base; 00201 } u_attempts; 00202 u_attempts.base = 0; 00203 u_attempts.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00204 u_attempts.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00205 u_attempts.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00206 u_attempts.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00207 this->attempts = u_attempts.real; 00208 offset += sizeof(this->attempts); 00209 return offset; 00210 } 00211 00212 virtual const char * getType(){ return "moveit_msgs/PositionIKRequest"; }; 00213 virtual const char * getMD5(){ return "9936dc239c90af180ec94a51596c96f2"; }; 00214 00215 }; 00216 00217 } 00218 #endif
Generated on Mon Sep 26 2022 13:47:03 by
1.7.2