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
CollisionObject.h
00001 #ifndef _ROS_moveit_msgs_CollisionObject_h 00002 #define _ROS_moveit_msgs_CollisionObject_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "std_msgs/Header.h" 00009 #include "object_recognition_msgs/ObjectType.h" 00010 #include "shape_msgs/SolidPrimitive.h" 00011 #include "geometry_msgs/Pose.h" 00012 #include "shape_msgs/Mesh.h" 00013 #include "shape_msgs/Plane.h" 00014 00015 namespace moveit_msgs 00016 { 00017 00018 class CollisionObject : public ros::Msg 00019 { 00020 public: 00021 typedef std_msgs::Header _header_type; 00022 _header_type header; 00023 typedef const char* _id_type; 00024 _id_type id; 00025 typedef object_recognition_msgs::ObjectType _type_type; 00026 _type_type type; 00027 uint32_t primitives_length; 00028 typedef shape_msgs::SolidPrimitive _primitives_type; 00029 _primitives_type st_primitives; 00030 _primitives_type * primitives; 00031 uint32_t primitive_poses_length; 00032 typedef geometry_msgs::Pose _primitive_poses_type; 00033 _primitive_poses_type st_primitive_poses; 00034 _primitive_poses_type * primitive_poses; 00035 uint32_t meshes_length; 00036 typedef shape_msgs::Mesh _meshes_type; 00037 _meshes_type st_meshes; 00038 _meshes_type * meshes; 00039 uint32_t mesh_poses_length; 00040 typedef geometry_msgs::Pose _mesh_poses_type; 00041 _mesh_poses_type st_mesh_poses; 00042 _mesh_poses_type * mesh_poses; 00043 uint32_t planes_length; 00044 typedef shape_msgs::Plane _planes_type; 00045 _planes_type st_planes; 00046 _planes_type * planes; 00047 uint32_t plane_poses_length; 00048 typedef geometry_msgs::Pose _plane_poses_type; 00049 _plane_poses_type st_plane_poses; 00050 _plane_poses_type * plane_poses; 00051 typedef int8_t _operation_type; 00052 _operation_type operation; 00053 enum { ADD = 0 }; 00054 enum { REMOVE = 1 }; 00055 enum { APPEND = 2 }; 00056 enum { MOVE = 3 }; 00057 00058 CollisionObject(): 00059 header(), 00060 id(""), 00061 type(), 00062 primitives_length(0), primitives(NULL), 00063 primitive_poses_length(0), primitive_poses(NULL), 00064 meshes_length(0), meshes(NULL), 00065 mesh_poses_length(0), mesh_poses(NULL), 00066 planes_length(0), planes(NULL), 00067 plane_poses_length(0), plane_poses(NULL), 00068 operation(0) 00069 { 00070 } 00071 00072 virtual int serialize(unsigned char *outbuffer) const 00073 { 00074 int offset = 0; 00075 offset += this->header.serialize(outbuffer + offset); 00076 uint32_t length_id = strlen(this->id); 00077 varToArr(outbuffer + offset, length_id); 00078 offset += 4; 00079 memcpy(outbuffer + offset, this->id, length_id); 00080 offset += length_id; 00081 offset += this->type.serialize(outbuffer + offset); 00082 *(outbuffer + offset + 0) = (this->primitives_length >> (8 * 0)) & 0xFF; 00083 *(outbuffer + offset + 1) = (this->primitives_length >> (8 * 1)) & 0xFF; 00084 *(outbuffer + offset + 2) = (this->primitives_length >> (8 * 2)) & 0xFF; 00085 *(outbuffer + offset + 3) = (this->primitives_length >> (8 * 3)) & 0xFF; 00086 offset += sizeof(this->primitives_length); 00087 for( uint32_t i = 0; i < primitives_length; i++){ 00088 offset += this->primitives[i].serialize(outbuffer + offset); 00089 } 00090 *(outbuffer + offset + 0) = (this->primitive_poses_length >> (8 * 0)) & 0xFF; 00091 *(outbuffer + offset + 1) = (this->primitive_poses_length >> (8 * 1)) & 0xFF; 00092 *(outbuffer + offset + 2) = (this->primitive_poses_length >> (8 * 2)) & 0xFF; 00093 *(outbuffer + offset + 3) = (this->primitive_poses_length >> (8 * 3)) & 0xFF; 00094 offset += sizeof(this->primitive_poses_length); 00095 for( uint32_t i = 0; i < primitive_poses_length; i++){ 00096 offset += this->primitive_poses[i].serialize(outbuffer + offset); 00097 } 00098 *(outbuffer + offset + 0) = (this->meshes_length >> (8 * 0)) & 0xFF; 00099 *(outbuffer + offset + 1) = (this->meshes_length >> (8 * 1)) & 0xFF; 00100 *(outbuffer + offset + 2) = (this->meshes_length >> (8 * 2)) & 0xFF; 00101 *(outbuffer + offset + 3) = (this->meshes_length >> (8 * 3)) & 0xFF; 00102 offset += sizeof(this->meshes_length); 00103 for( uint32_t i = 0; i < meshes_length; i++){ 00104 offset += this->meshes[i].serialize(outbuffer + offset); 00105 } 00106 *(outbuffer + offset + 0) = (this->mesh_poses_length >> (8 * 0)) & 0xFF; 00107 *(outbuffer + offset + 1) = (this->mesh_poses_length >> (8 * 1)) & 0xFF; 00108 *(outbuffer + offset + 2) = (this->mesh_poses_length >> (8 * 2)) & 0xFF; 00109 *(outbuffer + offset + 3) = (this->mesh_poses_length >> (8 * 3)) & 0xFF; 00110 offset += sizeof(this->mesh_poses_length); 00111 for( uint32_t i = 0; i < mesh_poses_length; i++){ 00112 offset += this->mesh_poses[i].serialize(outbuffer + offset); 00113 } 00114 *(outbuffer + offset + 0) = (this->planes_length >> (8 * 0)) & 0xFF; 00115 *(outbuffer + offset + 1) = (this->planes_length >> (8 * 1)) & 0xFF; 00116 *(outbuffer + offset + 2) = (this->planes_length >> (8 * 2)) & 0xFF; 00117 *(outbuffer + offset + 3) = (this->planes_length >> (8 * 3)) & 0xFF; 00118 offset += sizeof(this->planes_length); 00119 for( uint32_t i = 0; i < planes_length; i++){ 00120 offset += this->planes[i].serialize(outbuffer + offset); 00121 } 00122 *(outbuffer + offset + 0) = (this->plane_poses_length >> (8 * 0)) & 0xFF; 00123 *(outbuffer + offset + 1) = (this->plane_poses_length >> (8 * 1)) & 0xFF; 00124 *(outbuffer + offset + 2) = (this->plane_poses_length >> (8 * 2)) & 0xFF; 00125 *(outbuffer + offset + 3) = (this->plane_poses_length >> (8 * 3)) & 0xFF; 00126 offset += sizeof(this->plane_poses_length); 00127 for( uint32_t i = 0; i < plane_poses_length; i++){ 00128 offset += this->plane_poses[i].serialize(outbuffer + offset); 00129 } 00130 union { 00131 int8_t real; 00132 uint8_t base; 00133 } u_operation; 00134 u_operation.real = this->operation; 00135 *(outbuffer + offset + 0) = (u_operation.base >> (8 * 0)) & 0xFF; 00136 offset += sizeof(this->operation); 00137 return offset; 00138 } 00139 00140 virtual int deserialize(unsigned char *inbuffer) 00141 { 00142 int offset = 0; 00143 offset += this->header.deserialize(inbuffer + offset); 00144 uint32_t length_id; 00145 arrToVar(length_id, (inbuffer + offset)); 00146 offset += 4; 00147 for(unsigned int k= offset; k< offset+length_id; ++k){ 00148 inbuffer[k-1]=inbuffer[k]; 00149 } 00150 inbuffer[offset+length_id-1]=0; 00151 this->id = (char *)(inbuffer + offset-1); 00152 offset += length_id; 00153 offset += this->type.deserialize(inbuffer + offset); 00154 uint32_t primitives_lengthT = ((uint32_t) (*(inbuffer + offset))); 00155 primitives_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00156 primitives_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00157 primitives_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00158 offset += sizeof(this->primitives_length); 00159 if(primitives_lengthT > primitives_length) 00160 this->primitives = (shape_msgs::SolidPrimitive*)realloc(this->primitives, primitives_lengthT * sizeof(shape_msgs::SolidPrimitive)); 00161 primitives_length = primitives_lengthT; 00162 for( uint32_t i = 0; i < primitives_length; i++){ 00163 offset += this->st_primitives.deserialize(inbuffer + offset); 00164 memcpy( &(this->primitives[i]), &(this->st_primitives), sizeof(shape_msgs::SolidPrimitive)); 00165 } 00166 uint32_t primitive_poses_lengthT = ((uint32_t) (*(inbuffer + offset))); 00167 primitive_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00168 primitive_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00169 primitive_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00170 offset += sizeof(this->primitive_poses_length); 00171 if(primitive_poses_lengthT > primitive_poses_length) 00172 this->primitive_poses = (geometry_msgs::Pose*)realloc(this->primitive_poses, primitive_poses_lengthT * sizeof(geometry_msgs::Pose)); 00173 primitive_poses_length = primitive_poses_lengthT; 00174 for( uint32_t i = 0; i < primitive_poses_length; i++){ 00175 offset += this->st_primitive_poses.deserialize(inbuffer + offset); 00176 memcpy( &(this->primitive_poses[i]), &(this->st_primitive_poses), sizeof(geometry_msgs::Pose)); 00177 } 00178 uint32_t meshes_lengthT = ((uint32_t) (*(inbuffer + offset))); 00179 meshes_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00180 meshes_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00181 meshes_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00182 offset += sizeof(this->meshes_length); 00183 if(meshes_lengthT > meshes_length) 00184 this->meshes = (shape_msgs::Mesh*)realloc(this->meshes, meshes_lengthT * sizeof(shape_msgs::Mesh)); 00185 meshes_length = meshes_lengthT; 00186 for( uint32_t i = 0; i < meshes_length; i++){ 00187 offset += this->st_meshes.deserialize(inbuffer + offset); 00188 memcpy( &(this->meshes[i]), &(this->st_meshes), sizeof(shape_msgs::Mesh)); 00189 } 00190 uint32_t mesh_poses_lengthT = ((uint32_t) (*(inbuffer + offset))); 00191 mesh_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00192 mesh_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00193 mesh_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00194 offset += sizeof(this->mesh_poses_length); 00195 if(mesh_poses_lengthT > mesh_poses_length) 00196 this->mesh_poses = (geometry_msgs::Pose*)realloc(this->mesh_poses, mesh_poses_lengthT * sizeof(geometry_msgs::Pose)); 00197 mesh_poses_length = mesh_poses_lengthT; 00198 for( uint32_t i = 0; i < mesh_poses_length; i++){ 00199 offset += this->st_mesh_poses.deserialize(inbuffer + offset); 00200 memcpy( &(this->mesh_poses[i]), &(this->st_mesh_poses), sizeof(geometry_msgs::Pose)); 00201 } 00202 uint32_t planes_lengthT = ((uint32_t) (*(inbuffer + offset))); 00203 planes_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00204 planes_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00205 planes_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00206 offset += sizeof(this->planes_length); 00207 if(planes_lengthT > planes_length) 00208 this->planes = (shape_msgs::Plane*)realloc(this->planes, planes_lengthT * sizeof(shape_msgs::Plane)); 00209 planes_length = planes_lengthT; 00210 for( uint32_t i = 0; i < planes_length; i++){ 00211 offset += this->st_planes.deserialize(inbuffer + offset); 00212 memcpy( &(this->planes[i]), &(this->st_planes), sizeof(shape_msgs::Plane)); 00213 } 00214 uint32_t plane_poses_lengthT = ((uint32_t) (*(inbuffer + offset))); 00215 plane_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00216 plane_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00217 plane_poses_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00218 offset += sizeof(this->plane_poses_length); 00219 if(plane_poses_lengthT > plane_poses_length) 00220 this->plane_poses = (geometry_msgs::Pose*)realloc(this->plane_poses, plane_poses_lengthT * sizeof(geometry_msgs::Pose)); 00221 plane_poses_length = plane_poses_lengthT; 00222 for( uint32_t i = 0; i < plane_poses_length; i++){ 00223 offset += this->st_plane_poses.deserialize(inbuffer + offset); 00224 memcpy( &(this->plane_poses[i]), &(this->st_plane_poses), sizeof(geometry_msgs::Pose)); 00225 } 00226 union { 00227 int8_t real; 00228 uint8_t base; 00229 } u_operation; 00230 u_operation.base = 0; 00231 u_operation.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00232 this->operation = u_operation.real; 00233 offset += sizeof(this->operation); 00234 return offset; 00235 } 00236 00237 virtual const char * getType(){ return "moveit_msgs/CollisionObject"; }; 00238 virtual const char * getMD5(){ return "568a161b26dc46c54a5a07621ce82cf3"; }; 00239 00240 }; 00241 00242 } 00243 #endif
Generated on Mon Sep 26 2022 13:47:00 by
1.7.2