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
Grasp.h
00001 #ifndef _ROS_moveit_msgs_Grasp_h 00002 #define _ROS_moveit_msgs_Grasp_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "trajectory_msgs/JointTrajectory.h" 00009 #include "geometry_msgs/PoseStamped.h" 00010 #include "moveit_msgs/GripperTranslation.h" 00011 00012 namespace moveit_msgs 00013 { 00014 00015 class Grasp : public ros::Msg 00016 { 00017 public: 00018 typedef const char* _id_type; 00019 _id_type id; 00020 typedef trajectory_msgs::JointTrajectory _pre_grasp_posture_type; 00021 _pre_grasp_posture_type pre_grasp_posture; 00022 typedef trajectory_msgs::JointTrajectory _grasp_posture_type; 00023 _grasp_posture_type grasp_posture; 00024 typedef geometry_msgs::PoseStamped _grasp_pose_type; 00025 _grasp_pose_type grasp_pose; 00026 typedef double _grasp_quality_type; 00027 _grasp_quality_type grasp_quality; 00028 typedef moveit_msgs::GripperTranslation _pre_grasp_approach_type; 00029 _pre_grasp_approach_type pre_grasp_approach; 00030 typedef moveit_msgs::GripperTranslation _post_grasp_retreat_type; 00031 _post_grasp_retreat_type post_grasp_retreat; 00032 typedef moveit_msgs::GripperTranslation _post_place_retreat_type; 00033 _post_place_retreat_type post_place_retreat; 00034 typedef float _max_contact_force_type; 00035 _max_contact_force_type max_contact_force; 00036 uint32_t allowed_touch_objects_length; 00037 typedef char* _allowed_touch_objects_type; 00038 _allowed_touch_objects_type st_allowed_touch_objects; 00039 _allowed_touch_objects_type * allowed_touch_objects; 00040 00041 Grasp(): 00042 id(""), 00043 pre_grasp_posture(), 00044 grasp_posture(), 00045 grasp_pose(), 00046 grasp_quality(0), 00047 pre_grasp_approach(), 00048 post_grasp_retreat(), 00049 post_place_retreat(), 00050 max_contact_force(0), 00051 allowed_touch_objects_length(0), allowed_touch_objects(NULL) 00052 { 00053 } 00054 00055 virtual int serialize(unsigned char *outbuffer) const 00056 { 00057 int offset = 0; 00058 uint32_t length_id = strlen(this->id); 00059 varToArr(outbuffer + offset, length_id); 00060 offset += 4; 00061 memcpy(outbuffer + offset, this->id, length_id); 00062 offset += length_id; 00063 offset += this->pre_grasp_posture.serialize(outbuffer + offset); 00064 offset += this->grasp_posture.serialize(outbuffer + offset); 00065 offset += this->grasp_pose.serialize(outbuffer + offset); 00066 union { 00067 double real; 00068 uint64_t base; 00069 } u_grasp_quality; 00070 u_grasp_quality.real = this->grasp_quality; 00071 *(outbuffer + offset + 0) = (u_grasp_quality.base >> (8 * 0)) & 0xFF; 00072 *(outbuffer + offset + 1) = (u_grasp_quality.base >> (8 * 1)) & 0xFF; 00073 *(outbuffer + offset + 2) = (u_grasp_quality.base >> (8 * 2)) & 0xFF; 00074 *(outbuffer + offset + 3) = (u_grasp_quality.base >> (8 * 3)) & 0xFF; 00075 *(outbuffer + offset + 4) = (u_grasp_quality.base >> (8 * 4)) & 0xFF; 00076 *(outbuffer + offset + 5) = (u_grasp_quality.base >> (8 * 5)) & 0xFF; 00077 *(outbuffer + offset + 6) = (u_grasp_quality.base >> (8 * 6)) & 0xFF; 00078 *(outbuffer + offset + 7) = (u_grasp_quality.base >> (8 * 7)) & 0xFF; 00079 offset += sizeof(this->grasp_quality); 00080 offset += this->pre_grasp_approach.serialize(outbuffer + offset); 00081 offset += this->post_grasp_retreat.serialize(outbuffer + offset); 00082 offset += this->post_place_retreat.serialize(outbuffer + offset); 00083 union { 00084 float real; 00085 uint32_t base; 00086 } u_max_contact_force; 00087 u_max_contact_force.real = this->max_contact_force; 00088 *(outbuffer + offset + 0) = (u_max_contact_force.base >> (8 * 0)) & 0xFF; 00089 *(outbuffer + offset + 1) = (u_max_contact_force.base >> (8 * 1)) & 0xFF; 00090 *(outbuffer + offset + 2) = (u_max_contact_force.base >> (8 * 2)) & 0xFF; 00091 *(outbuffer + offset + 3) = (u_max_contact_force.base >> (8 * 3)) & 0xFF; 00092 offset += sizeof(this->max_contact_force); 00093 *(outbuffer + offset + 0) = (this->allowed_touch_objects_length >> (8 * 0)) & 0xFF; 00094 *(outbuffer + offset + 1) = (this->allowed_touch_objects_length >> (8 * 1)) & 0xFF; 00095 *(outbuffer + offset + 2) = (this->allowed_touch_objects_length >> (8 * 2)) & 0xFF; 00096 *(outbuffer + offset + 3) = (this->allowed_touch_objects_length >> (8 * 3)) & 0xFF; 00097 offset += sizeof(this->allowed_touch_objects_length); 00098 for( uint32_t i = 0; i < allowed_touch_objects_length; i++){ 00099 uint32_t length_allowed_touch_objectsi = strlen(this->allowed_touch_objects[i]); 00100 varToArr(outbuffer + offset, length_allowed_touch_objectsi); 00101 offset += 4; 00102 memcpy(outbuffer + offset, this->allowed_touch_objects[i], length_allowed_touch_objectsi); 00103 offset += length_allowed_touch_objectsi; 00104 } 00105 return offset; 00106 } 00107 00108 virtual int deserialize(unsigned char *inbuffer) 00109 { 00110 int offset = 0; 00111 uint32_t length_id; 00112 arrToVar(length_id, (inbuffer + offset)); 00113 offset += 4; 00114 for(unsigned int k= offset; k< offset+length_id; ++k){ 00115 inbuffer[k-1]=inbuffer[k]; 00116 } 00117 inbuffer[offset+length_id-1]=0; 00118 this->id = (char *)(inbuffer + offset-1); 00119 offset += length_id; 00120 offset += this->pre_grasp_posture.deserialize(inbuffer + offset); 00121 offset += this->grasp_posture.deserialize(inbuffer + offset); 00122 offset += this->grasp_pose.deserialize(inbuffer + offset); 00123 union { 00124 double real; 00125 uint64_t base; 00126 } u_grasp_quality; 00127 u_grasp_quality.base = 0; 00128 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00129 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00130 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00131 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00132 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00133 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00134 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00135 u_grasp_quality.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00136 this->grasp_quality = u_grasp_quality.real; 00137 offset += sizeof(this->grasp_quality); 00138 offset += this->pre_grasp_approach.deserialize(inbuffer + offset); 00139 offset += this->post_grasp_retreat.deserialize(inbuffer + offset); 00140 offset += this->post_place_retreat.deserialize(inbuffer + offset); 00141 union { 00142 float real; 00143 uint32_t base; 00144 } u_max_contact_force; 00145 u_max_contact_force.base = 0; 00146 u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00147 u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00148 u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00149 u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00150 this->max_contact_force = u_max_contact_force.real; 00151 offset += sizeof(this->max_contact_force); 00152 uint32_t allowed_touch_objects_lengthT = ((uint32_t) (*(inbuffer + offset))); 00153 allowed_touch_objects_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00154 allowed_touch_objects_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00155 allowed_touch_objects_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00156 offset += sizeof(this->allowed_touch_objects_length); 00157 if(allowed_touch_objects_lengthT > allowed_touch_objects_length) 00158 this->allowed_touch_objects = (char**)realloc(this->allowed_touch_objects, allowed_touch_objects_lengthT * sizeof(char*)); 00159 allowed_touch_objects_length = allowed_touch_objects_lengthT; 00160 for( uint32_t i = 0; i < allowed_touch_objects_length; i++){ 00161 uint32_t length_st_allowed_touch_objects; 00162 arrToVar(length_st_allowed_touch_objects, (inbuffer + offset)); 00163 offset += 4; 00164 for(unsigned int k= offset; k< offset+length_st_allowed_touch_objects; ++k){ 00165 inbuffer[k-1]=inbuffer[k]; 00166 } 00167 inbuffer[offset+length_st_allowed_touch_objects-1]=0; 00168 this->st_allowed_touch_objects = (char *)(inbuffer + offset-1); 00169 offset += length_st_allowed_touch_objects; 00170 memcpy( &(this->allowed_touch_objects[i]), &(this->st_allowed_touch_objects), sizeof(char*)); 00171 } 00172 return offset; 00173 } 00174 00175 virtual const char * getType(){ return "moveit_msgs/Grasp"; }; 00176 virtual const char * getMD5(){ return "e26c8fb64f589c33c5d5e54bd7b5e4cb"; }; 00177 00178 }; 00179 00180 } 00181 #endif
Generated on Mon Sep 26 2022 13:47:01 by
1.7.2