00001 #ifndef _ROS_manipulation_msgs_SceneRegion_h 00002 #define _ROS_manipulation_msgs_SceneRegion_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "sensor_msgs/PointCloud2.h" 00009 #include "sensor_msgs/Image.h" 00010 #include "sensor_msgs/CameraInfo.h" 00011 #include "geometry_msgs/PoseStamped.h" 00012 #include "geometry_msgs/Vector3.h" 00013 00014 namespace manipulation_msgs 00015 { 00016 00017 class SceneRegion : public ros::Msg 00018 { 00019 public: 00020 sensor_msgs::PointCloud2 cloud; 00021 uint8_t mask_length; 00022 int32_t st_mask; 00023 int32_t * mask; 00024 sensor_msgs::Image image; 00025 sensor_msgs::Image disparity_image; 00026 sensor_msgs::CameraInfo cam_info; 00027 geometry_msgs::PoseStamped roi_box_pose; 00028 geometry_msgs::Vector3 roi_box_dims; 00029 00030 virtual int serialize(unsigned char *outbuffer) const 00031 { 00032 int offset = 0; 00033 offset += this->cloud.serialize(outbuffer + offset); 00034 *(outbuffer + offset++) = mask_length; 00035 *(outbuffer + offset++) = 0; 00036 *(outbuffer + offset++) = 0; 00037 *(outbuffer + offset++) = 0; 00038 for( uint8_t i = 0; i < mask_length; i++){ 00039 union { 00040 int32_t real; 00041 uint32_t base; 00042 } u_maski; 00043 u_maski.real = this->mask[i]; 00044 *(outbuffer + offset + 0) = (u_maski.base >> (8 * 0)) & 0xFF; 00045 *(outbuffer + offset + 1) = (u_maski.base >> (8 * 1)) & 0xFF; 00046 *(outbuffer + offset + 2) = (u_maski.base >> (8 * 2)) & 0xFF; 00047 *(outbuffer + offset + 3) = (u_maski.base >> (8 * 3)) & 0xFF; 00048 offset += sizeof(this->mask[i]); 00049 } 00050 offset += this->image.serialize(outbuffer + offset); 00051 offset += this->disparity_image.serialize(outbuffer + offset); 00052 offset += this->cam_info.serialize(outbuffer + offset); 00053 offset += this->roi_box_pose.serialize(outbuffer + offset); 00054 offset += this->roi_box_dims.serialize(outbuffer + offset); 00055 return offset; 00056 } 00057 00058 virtual int deserialize(unsigned char *inbuffer) 00059 { 00060 int offset = 0; 00061 offset += this->cloud.deserialize(inbuffer + offset); 00062 uint8_t mask_lengthT = *(inbuffer + offset++); 00063 if(mask_lengthT > mask_length) 00064 this->mask = (int32_t*)realloc(this->mask, mask_lengthT * sizeof(int32_t)); 00065 offset += 3; 00066 mask_length = mask_lengthT; 00067 for( uint8_t i = 0; i < mask_length; i++){ 00068 union { 00069 int32_t real; 00070 uint32_t base; 00071 } u_st_mask; 00072 u_st_mask.base = 0; 00073 u_st_mask.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); 00074 u_st_mask.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00075 u_st_mask.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00076 u_st_mask.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00077 this->st_mask = u_st_mask.real; 00078 offset += sizeof(this->st_mask); 00079 memcpy( &(this->mask[i]), &(this->st_mask), sizeof(int32_t)); 00080 } 00081 offset += this->image.deserialize(inbuffer + offset); 00082 offset += this->disparity_image.deserialize(inbuffer + offset); 00083 offset += this->cam_info.deserialize(inbuffer + offset); 00084 offset += this->roi_box_pose.deserialize(inbuffer + offset); 00085 offset += this->roi_box_dims.deserialize(inbuffer + offset); 00086 return offset; 00087 } 00088 00089 const char * getType(){ return "manipulation_msgs/SceneRegion"; }; 00090 const char * getMD5(){ return "0a9ab02b19292633619876c9d247690c"; }; 00091 00092 }; 00093 00094 } 00095 #endif