00001 #ifndef _ROS_moveit_msgs_PickupGoal_h 00002 #define _ROS_moveit_msgs_PickupGoal_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "moveit_msgs/Grasp.h" 00009 #include "moveit_msgs/Constraints.h" 00010 #include "moveit_msgs/PlanningOptions.h" 00011 00012 namespace moveit_msgs 00013 { 00014 00015 class PickupGoal : public ros::Msg 00016 { 00017 public: 00018 const char* target_name; 00019 const char* group_name; 00020 const char* end_effector; 00021 uint8_t possible_grasps_length; 00022 moveit_msgs::Grasp st_possible_grasps; 00023 moveit_msgs::Grasp * possible_grasps; 00024 const char* support_surface_name; 00025 bool allow_gripper_support_collision; 00026 uint8_t attached_object_touch_links_length; 00027 char* st_attached_object_touch_links; 00028 char* * attached_object_touch_links; 00029 bool minimize_object_distance; 00030 moveit_msgs::Constraints path_constraints; 00031 const char* planner_id; 00032 uint8_t allowed_touch_objects_length; 00033 char* st_allowed_touch_objects; 00034 char* * allowed_touch_objects; 00035 float allowed_planning_time; 00036 moveit_msgs::PlanningOptions planning_options; 00037 00038 virtual int serialize(unsigned char *outbuffer) const 00039 { 00040 int offset = 0; 00041 uint32_t length_target_name = strlen(this->target_name); 00042 memcpy(outbuffer + offset, &length_target_name, sizeof(uint32_t)); 00043 offset += 4; 00044 memcpy(outbuffer + offset, this->target_name, length_target_name); 00045 offset += length_target_name; 00046 uint32_t length_group_name = strlen(this->group_name); 00047 memcpy(outbuffer + offset, &length_group_name, sizeof(uint32_t)); 00048 offset += 4; 00049 memcpy(outbuffer + offset, this->group_name, length_group_name); 00050 offset += length_group_name; 00051 uint32_t length_end_effector = strlen(this->end_effector); 00052 memcpy(outbuffer + offset, &length_end_effector, sizeof(uint32_t)); 00053 offset += 4; 00054 memcpy(outbuffer + offset, this->end_effector, length_end_effector); 00055 offset += length_end_effector; 00056 *(outbuffer + offset++) = possible_grasps_length; 00057 *(outbuffer + offset++) = 0; 00058 *(outbuffer + offset++) = 0; 00059 *(outbuffer + offset++) = 0; 00060 for( uint8_t i = 0; i < possible_grasps_length; i++){ 00061 offset += this->possible_grasps[i].serialize(outbuffer + offset); 00062 } 00063 uint32_t length_support_surface_name = strlen(this->support_surface_name); 00064 memcpy(outbuffer + offset, &length_support_surface_name, sizeof(uint32_t)); 00065 offset += 4; 00066 memcpy(outbuffer + offset, this->support_surface_name, length_support_surface_name); 00067 offset += length_support_surface_name; 00068 union { 00069 bool real; 00070 uint8_t base; 00071 } u_allow_gripper_support_collision; 00072 u_allow_gripper_support_collision.real = this->allow_gripper_support_collision; 00073 *(outbuffer + offset + 0) = (u_allow_gripper_support_collision.base >> (8 * 0)) & 0xFF; 00074 offset += sizeof(this->allow_gripper_support_collision); 00075 *(outbuffer + offset++) = attached_object_touch_links_length; 00076 *(outbuffer + offset++) = 0; 00077 *(outbuffer + offset++) = 0; 00078 *(outbuffer + offset++) = 0; 00079 for( uint8_t i = 0; i < attached_object_touch_links_length; i++){ 00080 uint32_t length_attached_object_touch_linksi = strlen(this->attached_object_touch_links[i]); 00081 memcpy(outbuffer + offset, &length_attached_object_touch_linksi, sizeof(uint32_t)); 00082 offset += 4; 00083 memcpy(outbuffer + offset, this->attached_object_touch_links[i], length_attached_object_touch_linksi); 00084 offset += length_attached_object_touch_linksi; 00085 } 00086 union { 00087 bool real; 00088 uint8_t base; 00089 } u_minimize_object_distance; 00090 u_minimize_object_distance.real = this->minimize_object_distance; 00091 *(outbuffer + offset + 0) = (u_minimize_object_distance.base >> (8 * 0)) & 0xFF; 00092 offset += sizeof(this->minimize_object_distance); 00093 offset += this->path_constraints.serialize(outbuffer + offset); 00094 uint32_t length_planner_id = strlen(this->planner_id); 00095 memcpy(outbuffer + offset, &length_planner_id, sizeof(uint32_t)); 00096 offset += 4; 00097 memcpy(outbuffer + offset, this->planner_id, length_planner_id); 00098 offset += length_planner_id; 00099 *(outbuffer + offset++) = allowed_touch_objects_length; 00100 *(outbuffer + offset++) = 0; 00101 *(outbuffer + offset++) = 0; 00102 *(outbuffer + offset++) = 0; 00103 for( uint8_t i = 0; i < allowed_touch_objects_length; i++){ 00104 uint32_t length_allowed_touch_objectsi = strlen(this->allowed_touch_objects[i]); 00105 memcpy(outbuffer + offset, &length_allowed_touch_objectsi, sizeof(uint32_t)); 00106 offset += 4; 00107 memcpy(outbuffer + offset, this->allowed_touch_objects[i], length_allowed_touch_objectsi); 00108 offset += length_allowed_touch_objectsi; 00109 } 00110 int32_t * val_allowed_planning_time = (int32_t *) &(this->allowed_planning_time); 00111 int32_t exp_allowed_planning_time = (((*val_allowed_planning_time)>>23)&255); 00112 if(exp_allowed_planning_time != 0) 00113 exp_allowed_planning_time += 1023-127; 00114 int32_t sig_allowed_planning_time = *val_allowed_planning_time; 00115 *(outbuffer + offset++) = 0; 00116 *(outbuffer + offset++) = 0; 00117 *(outbuffer + offset++) = 0; 00118 *(outbuffer + offset++) = (sig_allowed_planning_time<<5) & 0xff; 00119 *(outbuffer + offset++) = (sig_allowed_planning_time>>3) & 0xff; 00120 *(outbuffer + offset++) = (sig_allowed_planning_time>>11) & 0xff; 00121 *(outbuffer + offset++) = ((exp_allowed_planning_time<<4) & 0xF0) | ((sig_allowed_planning_time>>19)&0x0F); 00122 *(outbuffer + offset++) = (exp_allowed_planning_time>>4) & 0x7F; 00123 if(this->allowed_planning_time < 0) *(outbuffer + offset -1) |= 0x80; 00124 offset += this->planning_options.serialize(outbuffer + offset); 00125 return offset; 00126 } 00127 00128 virtual int deserialize(unsigned char *inbuffer) 00129 { 00130 int offset = 0; 00131 uint32_t length_target_name; 00132 memcpy(&length_target_name, (inbuffer + offset), sizeof(uint32_t)); 00133 offset += 4; 00134 for(unsigned int k= offset; k< offset+length_target_name; ++k){ 00135 inbuffer[k-1]=inbuffer[k]; 00136 } 00137 inbuffer[offset+length_target_name-1]=0; 00138 this->target_name = (char *)(inbuffer + offset-1); 00139 offset += length_target_name; 00140 uint32_t length_group_name; 00141 memcpy(&length_group_name, (inbuffer + offset), sizeof(uint32_t)); 00142 offset += 4; 00143 for(unsigned int k= offset; k< offset+length_group_name; ++k){ 00144 inbuffer[k-1]=inbuffer[k]; 00145 } 00146 inbuffer[offset+length_group_name-1]=0; 00147 this->group_name = (char *)(inbuffer + offset-1); 00148 offset += length_group_name; 00149 uint32_t length_end_effector; 00150 memcpy(&length_end_effector, (inbuffer + offset), sizeof(uint32_t)); 00151 offset += 4; 00152 for(unsigned int k= offset; k< offset+length_end_effector; ++k){ 00153 inbuffer[k-1]=inbuffer[k]; 00154 } 00155 inbuffer[offset+length_end_effector-1]=0; 00156 this->end_effector = (char *)(inbuffer + offset-1); 00157 offset += length_end_effector; 00158 uint8_t possible_grasps_lengthT = *(inbuffer + offset++); 00159 if(possible_grasps_lengthT > possible_grasps_length) 00160 this->possible_grasps = (moveit_msgs::Grasp*)realloc(this->possible_grasps, possible_grasps_lengthT * sizeof(moveit_msgs::Grasp)); 00161 offset += 3; 00162 possible_grasps_length = possible_grasps_lengthT; 00163 for( uint8_t i = 0; i < possible_grasps_length; i++){ 00164 offset += this->st_possible_grasps.deserialize(inbuffer + offset); 00165 memcpy( &(this->possible_grasps[i]), &(this->st_possible_grasps), sizeof(moveit_msgs::Grasp)); 00166 } 00167 uint32_t length_support_surface_name; 00168 memcpy(&length_support_surface_name, (inbuffer + offset), sizeof(uint32_t)); 00169 offset += 4; 00170 for(unsigned int k= offset; k< offset+length_support_surface_name; ++k){ 00171 inbuffer[k-1]=inbuffer[k]; 00172 } 00173 inbuffer[offset+length_support_surface_name-1]=0; 00174 this->support_surface_name = (char *)(inbuffer + offset-1); 00175 offset += length_support_surface_name; 00176 union { 00177 bool real; 00178 uint8_t base; 00179 } u_allow_gripper_support_collision; 00180 u_allow_gripper_support_collision.base = 0; 00181 u_allow_gripper_support_collision.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00182 this->allow_gripper_support_collision = u_allow_gripper_support_collision.real; 00183 offset += sizeof(this->allow_gripper_support_collision); 00184 uint8_t attached_object_touch_links_lengthT = *(inbuffer + offset++); 00185 if(attached_object_touch_links_lengthT > attached_object_touch_links_length) 00186 this->attached_object_touch_links = (char**)realloc(this->attached_object_touch_links, attached_object_touch_links_lengthT * sizeof(char*)); 00187 offset += 3; 00188 attached_object_touch_links_length = attached_object_touch_links_lengthT; 00189 for( uint8_t i = 0; i < attached_object_touch_links_length; i++){ 00190 uint32_t length_st_attached_object_touch_links; 00191 memcpy(&length_st_attached_object_touch_links, (inbuffer + offset), sizeof(uint32_t)); 00192 offset += 4; 00193 for(unsigned int k= offset; k< offset+length_st_attached_object_touch_links; ++k){ 00194 inbuffer[k-1]=inbuffer[k]; 00195 } 00196 inbuffer[offset+length_st_attached_object_touch_links-1]=0; 00197 this->st_attached_object_touch_links = (char *)(inbuffer + offset-1); 00198 offset += length_st_attached_object_touch_links; 00199 memcpy( &(this->attached_object_touch_links[i]), &(this->st_attached_object_touch_links), sizeof(char*)); 00200 } 00201 union { 00202 bool real; 00203 uint8_t base; 00204 } u_minimize_object_distance; 00205 u_minimize_object_distance.base = 0; 00206 u_minimize_object_distance.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00207 this->minimize_object_distance = u_minimize_object_distance.real; 00208 offset += sizeof(this->minimize_object_distance); 00209 offset += this->path_constraints.deserialize(inbuffer + offset); 00210 uint32_t length_planner_id; 00211 memcpy(&length_planner_id, (inbuffer + offset), sizeof(uint32_t)); 00212 offset += 4; 00213 for(unsigned int k= offset; k< offset+length_planner_id; ++k){ 00214 inbuffer[k-1]=inbuffer[k]; 00215 } 00216 inbuffer[offset+length_planner_id-1]=0; 00217 this->planner_id = (char *)(inbuffer + offset-1); 00218 offset += length_planner_id; 00219 uint8_t allowed_touch_objects_lengthT = *(inbuffer + offset++); 00220 if(allowed_touch_objects_lengthT > allowed_touch_objects_length) 00221 this->allowed_touch_objects = (char**)realloc(this->allowed_touch_objects, allowed_touch_objects_lengthT * sizeof(char*)); 00222 offset += 3; 00223 allowed_touch_objects_length = allowed_touch_objects_lengthT; 00224 for( uint8_t i = 0; i < allowed_touch_objects_length; i++){ 00225 uint32_t length_st_allowed_touch_objects; 00226 memcpy(&length_st_allowed_touch_objects, (inbuffer + offset), sizeof(uint32_t)); 00227 offset += 4; 00228 for(unsigned int k= offset; k< offset+length_st_allowed_touch_objects; ++k){ 00229 inbuffer[k-1]=inbuffer[k]; 00230 } 00231 inbuffer[offset+length_st_allowed_touch_objects-1]=0; 00232 this->st_allowed_touch_objects = (char *)(inbuffer + offset-1); 00233 offset += length_st_allowed_touch_objects; 00234 memcpy( &(this->allowed_touch_objects[i]), &(this->st_allowed_touch_objects), sizeof(char*)); 00235 } 00236 uint32_t * val_allowed_planning_time = (uint32_t*) &(this->allowed_planning_time); 00237 offset += 3; 00238 *val_allowed_planning_time = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00239 *val_allowed_planning_time |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00240 *val_allowed_planning_time |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00241 *val_allowed_planning_time |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00242 uint32_t exp_allowed_planning_time = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00243 exp_allowed_planning_time |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00244 if(exp_allowed_planning_time !=0) 00245 *val_allowed_planning_time |= ((exp_allowed_planning_time)-1023+127)<<23; 00246 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->allowed_planning_time = -this->allowed_planning_time; 00247 offset += this->planning_options.deserialize(inbuffer + offset); 00248 return offset; 00249 } 00250 00251 const char * getType(){ return "moveit_msgs/PickupGoal"; }; 00252 const char * getMD5(){ return "458c6ab3761d73e99b070063f7b74c2a"; }; 00253 00254 }; 00255 00256 } 00257 #endif