Go to the documentation of this file.00001 #ifndef _ROS_manipulation_msgs_GraspableObject_h
00002 #define _ROS_manipulation_msgs_GraspableObject_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "household_objects_database_msgs/DatabaseModelPose.h"
00009 #include "sensor_msgs/PointCloud.h"
00010 #include "manipulation_msgs/SceneRegion.h"
00011
00012 namespace manipulation_msgs
00013 {
00014
00015 class GraspableObject : public ros::Msg
00016 {
00017 public:
00018 const char* reference_frame_id;
00019 uint8_t potential_models_length;
00020 household_objects_database_msgs::DatabaseModelPose st_potential_models;
00021 household_objects_database_msgs::DatabaseModelPose * potential_models;
00022 sensor_msgs::PointCloud cluster;
00023 manipulation_msgs::SceneRegion region;
00024 const char* collision_name;
00025
00026 virtual int serialize(unsigned char *outbuffer) const
00027 {
00028 int offset = 0;
00029 uint32_t length_reference_frame_id = strlen(this->reference_frame_id);
00030 memcpy(outbuffer + offset, &length_reference_frame_id, sizeof(uint32_t));
00031 offset += 4;
00032 memcpy(outbuffer + offset, this->reference_frame_id, length_reference_frame_id);
00033 offset += length_reference_frame_id;
00034 *(outbuffer + offset++) = potential_models_length;
00035 *(outbuffer + offset++) = 0;
00036 *(outbuffer + offset++) = 0;
00037 *(outbuffer + offset++) = 0;
00038 for( uint8_t i = 0; i < potential_models_length; i++){
00039 offset += this->potential_models[i].serialize(outbuffer + offset);
00040 }
00041 offset += this->cluster.serialize(outbuffer + offset);
00042 offset += this->region.serialize(outbuffer + offset);
00043 uint32_t length_collision_name = strlen(this->collision_name);
00044 memcpy(outbuffer + offset, &length_collision_name, sizeof(uint32_t));
00045 offset += 4;
00046 memcpy(outbuffer + offset, this->collision_name, length_collision_name);
00047 offset += length_collision_name;
00048 return offset;
00049 }
00050
00051 virtual int deserialize(unsigned char *inbuffer)
00052 {
00053 int offset = 0;
00054 uint32_t length_reference_frame_id;
00055 memcpy(&length_reference_frame_id, (inbuffer + offset), sizeof(uint32_t));
00056 offset += 4;
00057 for(unsigned int k= offset; k< offset+length_reference_frame_id; ++k){
00058 inbuffer[k-1]=inbuffer[k];
00059 }
00060 inbuffer[offset+length_reference_frame_id-1]=0;
00061 this->reference_frame_id = (char *)(inbuffer + offset-1);
00062 offset += length_reference_frame_id;
00063 uint8_t potential_models_lengthT = *(inbuffer + offset++);
00064 if(potential_models_lengthT > potential_models_length)
00065 this->potential_models = (household_objects_database_msgs::DatabaseModelPose*)realloc(this->potential_models, potential_models_lengthT * sizeof(household_objects_database_msgs::DatabaseModelPose));
00066 offset += 3;
00067 potential_models_length = potential_models_lengthT;
00068 for( uint8_t i = 0; i < potential_models_length; i++){
00069 offset += this->st_potential_models.deserialize(inbuffer + offset);
00070 memcpy( &(this->potential_models[i]), &(this->st_potential_models), sizeof(household_objects_database_msgs::DatabaseModelPose));
00071 }
00072 offset += this->cluster.deserialize(inbuffer + offset);
00073 offset += this->region.deserialize(inbuffer + offset);
00074 uint32_t length_collision_name;
00075 memcpy(&length_collision_name, (inbuffer + offset), sizeof(uint32_t));
00076 offset += 4;
00077 for(unsigned int k= offset; k< offset+length_collision_name; ++k){
00078 inbuffer[k-1]=inbuffer[k];
00079 }
00080 inbuffer[offset+length_collision_name-1]=0;
00081 this->collision_name = (char *)(inbuffer + offset-1);
00082 offset += length_collision_name;
00083 return offset;
00084 }
00085
00086 const char * getType(){ return "manipulation_msgs/GraspableObject"; };
00087 const char * getMD5(){ return "e2efd13d8e2bbb4697a5d71f167bceaa"; };
00088
00089 };
00090
00091 }
00092 #endif