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
MotionPlanRequest.h
00001 #ifndef _ROS_moveit_msgs_MotionPlanRequest_h 00002 #define _ROS_moveit_msgs_MotionPlanRequest_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "moveit_msgs/WorkspaceParameters.h" 00009 #include "moveit_msgs/RobotState.h" 00010 #include "moveit_msgs/Constraints.h" 00011 #include "moveit_msgs/TrajectoryConstraints.h" 00012 00013 namespace moveit_msgs 00014 { 00015 00016 class MotionPlanRequest : public ros::Msg 00017 { 00018 public: 00019 typedef moveit_msgs::WorkspaceParameters _workspace_parameters_type; 00020 _workspace_parameters_type workspace_parameters; 00021 typedef moveit_msgs::RobotState _start_state_type; 00022 _start_state_type start_state; 00023 uint32_t goal_constraints_length; 00024 typedef moveit_msgs::Constraints _goal_constraints_type; 00025 _goal_constraints_type st_goal_constraints; 00026 _goal_constraints_type * goal_constraints; 00027 typedef moveit_msgs::Constraints _path_constraints_type; 00028 _path_constraints_type path_constraints; 00029 typedef moveit_msgs::TrajectoryConstraints _trajectory_constraints_type; 00030 _trajectory_constraints_type trajectory_constraints; 00031 typedef const char* _planner_id_type; 00032 _planner_id_type planner_id; 00033 typedef const char* _group_name_type; 00034 _group_name_type group_name; 00035 typedef int32_t _num_planning_attempts_type; 00036 _num_planning_attempts_type num_planning_attempts; 00037 typedef double _allowed_planning_time_type; 00038 _allowed_planning_time_type allowed_planning_time; 00039 typedef double _max_velocity_scaling_factor_type; 00040 _max_velocity_scaling_factor_type max_velocity_scaling_factor; 00041 typedef double _max_acceleration_scaling_factor_type; 00042 _max_acceleration_scaling_factor_type max_acceleration_scaling_factor; 00043 00044 MotionPlanRequest(): 00045 workspace_parameters(), 00046 start_state(), 00047 goal_constraints_length(0), goal_constraints(NULL), 00048 path_constraints(), 00049 trajectory_constraints(), 00050 planner_id(""), 00051 group_name(""), 00052 num_planning_attempts(0), 00053 allowed_planning_time(0), 00054 max_velocity_scaling_factor(0), 00055 max_acceleration_scaling_factor(0) 00056 { 00057 } 00058 00059 virtual int serialize(unsigned char *outbuffer) const 00060 { 00061 int offset = 0; 00062 offset += this->workspace_parameters.serialize(outbuffer + offset); 00063 offset += this->start_state.serialize(outbuffer + offset); 00064 *(outbuffer + offset + 0) = (this->goal_constraints_length >> (8 * 0)) & 0xFF; 00065 *(outbuffer + offset + 1) = (this->goal_constraints_length >> (8 * 1)) & 0xFF; 00066 *(outbuffer + offset + 2) = (this->goal_constraints_length >> (8 * 2)) & 0xFF; 00067 *(outbuffer + offset + 3) = (this->goal_constraints_length >> (8 * 3)) & 0xFF; 00068 offset += sizeof(this->goal_constraints_length); 00069 for( uint32_t i = 0; i < goal_constraints_length; i++){ 00070 offset += this->goal_constraints[i].serialize(outbuffer + offset); 00071 } 00072 offset += this->path_constraints.serialize(outbuffer + offset); 00073 offset += this->trajectory_constraints.serialize(outbuffer + offset); 00074 uint32_t length_planner_id = strlen(this->planner_id); 00075 varToArr(outbuffer + offset, length_planner_id); 00076 offset += 4; 00077 memcpy(outbuffer + offset, this->planner_id, length_planner_id); 00078 offset += length_planner_id; 00079 uint32_t length_group_name = strlen(this->group_name); 00080 varToArr(outbuffer + offset, length_group_name); 00081 offset += 4; 00082 memcpy(outbuffer + offset, this->group_name, length_group_name); 00083 offset += length_group_name; 00084 union { 00085 int32_t real; 00086 uint32_t base; 00087 } u_num_planning_attempts; 00088 u_num_planning_attempts.real = this->num_planning_attempts; 00089 *(outbuffer + offset + 0) = (u_num_planning_attempts.base >> (8 * 0)) & 0xFF; 00090 *(outbuffer + offset + 1) = (u_num_planning_attempts.base >> (8 * 1)) & 0xFF; 00091 *(outbuffer + offset + 2) = (u_num_planning_attempts.base >> (8 * 2)) & 0xFF; 00092 *(outbuffer + offset + 3) = (u_num_planning_attempts.base >> (8 * 3)) & 0xFF; 00093 offset += sizeof(this->num_planning_attempts); 00094 union { 00095 double real; 00096 uint64_t base; 00097 } u_allowed_planning_time; 00098 u_allowed_planning_time.real = this->allowed_planning_time; 00099 *(outbuffer + offset + 0) = (u_allowed_planning_time.base >> (8 * 0)) & 0xFF; 00100 *(outbuffer + offset + 1) = (u_allowed_planning_time.base >> (8 * 1)) & 0xFF; 00101 *(outbuffer + offset + 2) = (u_allowed_planning_time.base >> (8 * 2)) & 0xFF; 00102 *(outbuffer + offset + 3) = (u_allowed_planning_time.base >> (8 * 3)) & 0xFF; 00103 *(outbuffer + offset + 4) = (u_allowed_planning_time.base >> (8 * 4)) & 0xFF; 00104 *(outbuffer + offset + 5) = (u_allowed_planning_time.base >> (8 * 5)) & 0xFF; 00105 *(outbuffer + offset + 6) = (u_allowed_planning_time.base >> (8 * 6)) & 0xFF; 00106 *(outbuffer + offset + 7) = (u_allowed_planning_time.base >> (8 * 7)) & 0xFF; 00107 offset += sizeof(this->allowed_planning_time); 00108 union { 00109 double real; 00110 uint64_t base; 00111 } u_max_velocity_scaling_factor; 00112 u_max_velocity_scaling_factor.real = this->max_velocity_scaling_factor; 00113 *(outbuffer + offset + 0) = (u_max_velocity_scaling_factor.base >> (8 * 0)) & 0xFF; 00114 *(outbuffer + offset + 1) = (u_max_velocity_scaling_factor.base >> (8 * 1)) & 0xFF; 00115 *(outbuffer + offset + 2) = (u_max_velocity_scaling_factor.base >> (8 * 2)) & 0xFF; 00116 *(outbuffer + offset + 3) = (u_max_velocity_scaling_factor.base >> (8 * 3)) & 0xFF; 00117 *(outbuffer + offset + 4) = (u_max_velocity_scaling_factor.base >> (8 * 4)) & 0xFF; 00118 *(outbuffer + offset + 5) = (u_max_velocity_scaling_factor.base >> (8 * 5)) & 0xFF; 00119 *(outbuffer + offset + 6) = (u_max_velocity_scaling_factor.base >> (8 * 6)) & 0xFF; 00120 *(outbuffer + offset + 7) = (u_max_velocity_scaling_factor.base >> (8 * 7)) & 0xFF; 00121 offset += sizeof(this->max_velocity_scaling_factor); 00122 union { 00123 double real; 00124 uint64_t base; 00125 } u_max_acceleration_scaling_factor; 00126 u_max_acceleration_scaling_factor.real = this->max_acceleration_scaling_factor; 00127 *(outbuffer + offset + 0) = (u_max_acceleration_scaling_factor.base >> (8 * 0)) & 0xFF; 00128 *(outbuffer + offset + 1) = (u_max_acceleration_scaling_factor.base >> (8 * 1)) & 0xFF; 00129 *(outbuffer + offset + 2) = (u_max_acceleration_scaling_factor.base >> (8 * 2)) & 0xFF; 00130 *(outbuffer + offset + 3) = (u_max_acceleration_scaling_factor.base >> (8 * 3)) & 0xFF; 00131 *(outbuffer + offset + 4) = (u_max_acceleration_scaling_factor.base >> (8 * 4)) & 0xFF; 00132 *(outbuffer + offset + 5) = (u_max_acceleration_scaling_factor.base >> (8 * 5)) & 0xFF; 00133 *(outbuffer + offset + 6) = (u_max_acceleration_scaling_factor.base >> (8 * 6)) & 0xFF; 00134 *(outbuffer + offset + 7) = (u_max_acceleration_scaling_factor.base >> (8 * 7)) & 0xFF; 00135 offset += sizeof(this->max_acceleration_scaling_factor); 00136 return offset; 00137 } 00138 00139 virtual int deserialize(unsigned char *inbuffer) 00140 { 00141 int offset = 0; 00142 offset += this->workspace_parameters.deserialize(inbuffer + offset); 00143 offset += this->start_state.deserialize(inbuffer + offset); 00144 uint32_t goal_constraints_lengthT = ((uint32_t) (*(inbuffer + offset))); 00145 goal_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00146 goal_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00147 goal_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00148 offset += sizeof(this->goal_constraints_length); 00149 if(goal_constraints_lengthT > goal_constraints_length) 00150 this->goal_constraints = (moveit_msgs::Constraints*)realloc(this->goal_constraints, goal_constraints_lengthT * sizeof(moveit_msgs::Constraints)); 00151 goal_constraints_length = goal_constraints_lengthT; 00152 for( uint32_t i = 0; i < goal_constraints_length; i++){ 00153 offset += this->st_goal_constraints.deserialize(inbuffer + offset); 00154 memcpy( &(this->goal_constraints[i]), &(this->st_goal_constraints), sizeof(moveit_msgs::Constraints)); 00155 } 00156 offset += this->path_constraints.deserialize(inbuffer + offset); 00157 offset += this->trajectory_constraints.deserialize(inbuffer + offset); 00158 uint32_t length_planner_id; 00159 arrToVar(length_planner_id, (inbuffer + offset)); 00160 offset += 4; 00161 for(unsigned int k= offset; k< offset+length_planner_id; ++k){ 00162 inbuffer[k-1]=inbuffer[k]; 00163 } 00164 inbuffer[offset+length_planner_id-1]=0; 00165 this->planner_id = (char *)(inbuffer + offset-1); 00166 offset += length_planner_id; 00167 uint32_t length_group_name; 00168 arrToVar(length_group_name, (inbuffer + offset)); 00169 offset += 4; 00170 for(unsigned int k= offset; k< offset+length_group_name; ++k){ 00171 inbuffer[k-1]=inbuffer[k]; 00172 } 00173 inbuffer[offset+length_group_name-1]=0; 00174 this->group_name = (char *)(inbuffer + offset-1); 00175 offset += length_group_name; 00176 union { 00177 int32_t real; 00178 uint32_t base; 00179 } u_num_planning_attempts; 00180 u_num_planning_attempts.base = 0; 00181 u_num_planning_attempts.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00182 u_num_planning_attempts.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00183 u_num_planning_attempts.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00184 u_num_planning_attempts.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00185 this->num_planning_attempts = u_num_planning_attempts.real; 00186 offset += sizeof(this->num_planning_attempts); 00187 union { 00188 double real; 00189 uint64_t base; 00190 } u_allowed_planning_time; 00191 u_allowed_planning_time.base = 0; 00192 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00193 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00194 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00195 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00196 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00197 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00198 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00199 u_allowed_planning_time.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00200 this->allowed_planning_time = u_allowed_planning_time.real; 00201 offset += sizeof(this->allowed_planning_time); 00202 union { 00203 double real; 00204 uint64_t base; 00205 } u_max_velocity_scaling_factor; 00206 u_max_velocity_scaling_factor.base = 0; 00207 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00208 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00209 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00210 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00211 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00212 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00213 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00214 u_max_velocity_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00215 this->max_velocity_scaling_factor = u_max_velocity_scaling_factor.real; 00216 offset += sizeof(this->max_velocity_scaling_factor); 00217 union { 00218 double real; 00219 uint64_t base; 00220 } u_max_acceleration_scaling_factor; 00221 u_max_acceleration_scaling_factor.base = 0; 00222 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00223 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00224 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00225 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00226 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00227 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00228 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00229 u_max_acceleration_scaling_factor.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00230 this->max_acceleration_scaling_factor = u_max_acceleration_scaling_factor.real; 00231 offset += sizeof(this->max_acceleration_scaling_factor); 00232 return offset; 00233 } 00234 00235 virtual const char * getType(){ return "moveit_msgs/MotionPlanRequest"; }; 00236 virtual const char * getMD5(){ return "c3bec13a525a6ae66e0fc57b768fdca6"; }; 00237 00238 }; 00239 00240 } 00241 #endif
Generated on Mon Sep 26 2022 13:47:02 by
1.7.2