00001 #ifndef _ROS_moveit_msgs_PlanningSceneWorld_h 00002 #define _ROS_moveit_msgs_PlanningSceneWorld_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "moveit_msgs/CollisionObject.h" 00009 #include "octomap_msgs/OctomapWithPose.h" 00010 00011 namespace moveit_msgs 00012 { 00013 00014 class PlanningSceneWorld : public ros::Msg 00015 { 00016 public: 00017 uint8_t collision_objects_length; 00018 moveit_msgs::CollisionObject st_collision_objects; 00019 moveit_msgs::CollisionObject * collision_objects; 00020 octomap_msgs::OctomapWithPose octomap; 00021 00022 virtual int serialize(unsigned char *outbuffer) const 00023 { 00024 int offset = 0; 00025 *(outbuffer + offset++) = collision_objects_length; 00026 *(outbuffer + offset++) = 0; 00027 *(outbuffer + offset++) = 0; 00028 *(outbuffer + offset++) = 0; 00029 for( uint8_t i = 0; i < collision_objects_length; i++){ 00030 offset += this->collision_objects[i].serialize(outbuffer + offset); 00031 } 00032 offset += this->octomap.serialize(outbuffer + offset); 00033 return offset; 00034 } 00035 00036 virtual int deserialize(unsigned char *inbuffer) 00037 { 00038 int offset = 0; 00039 uint8_t collision_objects_lengthT = *(inbuffer + offset++); 00040 if(collision_objects_lengthT > collision_objects_length) 00041 this->collision_objects = (moveit_msgs::CollisionObject*)realloc(this->collision_objects, collision_objects_lengthT * sizeof(moveit_msgs::CollisionObject)); 00042 offset += 3; 00043 collision_objects_length = collision_objects_lengthT; 00044 for( uint8_t i = 0; i < collision_objects_length; i++){ 00045 offset += this->st_collision_objects.deserialize(inbuffer + offset); 00046 memcpy( &(this->collision_objects[i]), &(this->st_collision_objects), sizeof(moveit_msgs::CollisionObject)); 00047 } 00048 offset += this->octomap.deserialize(inbuffer + offset); 00049 return offset; 00050 } 00051 00052 const char * getType(){ return "moveit_msgs/PlanningSceneWorld"; }; 00053 const char * getMD5(){ return "373d88390d1db385335639f687723ee6"; }; 00054 00055 }; 00056 00057 } 00058 #endif