PlanningScene.h

00001 #ifndef _ROS_moveit_msgs_PlanningScene_h
00002 #define _ROS_moveit_msgs_PlanningScene_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "moveit_msgs/RobotState.h"
00009 #include "geometry_msgs/TransformStamped.h"
00010 #include "moveit_msgs/AllowedCollisionMatrix.h"
00011 #include "moveit_msgs/LinkPadding.h"
00012 #include "moveit_msgs/LinkScale.h"
00013 #include "moveit_msgs/ObjectColor.h"
00014 #include "moveit_msgs/PlanningSceneWorld.h"
00015 
00016 namespace moveit_msgs
00017 {
00018 
00019   class PlanningScene : public ros::Msg
00020   {
00021     public:
00022       typedef const char* _name_type;
00023       _name_type name;
00024       typedef moveit_msgs::RobotState _robot_state_type;
00025       _robot_state_type robot_state;
00026       typedef const char* _robot_model_name_type;
00027       _robot_model_name_type robot_model_name;
00028       uint32_t fixed_frame_transforms_length;
00029       typedef geometry_msgs::TransformStamped _fixed_frame_transforms_type;
00030       _fixed_frame_transforms_type st_fixed_frame_transforms;
00031       _fixed_frame_transforms_type * fixed_frame_transforms;
00032       typedef moveit_msgs::AllowedCollisionMatrix _allowed_collision_matrix_type;
00033       _allowed_collision_matrix_type allowed_collision_matrix;
00034       uint32_t link_padding_length;
00035       typedef moveit_msgs::LinkPadding _link_padding_type;
00036       _link_padding_type st_link_padding;
00037       _link_padding_type * link_padding;
00038       uint32_t link_scale_length;
00039       typedef moveit_msgs::LinkScale _link_scale_type;
00040       _link_scale_type st_link_scale;
00041       _link_scale_type * link_scale;
00042       uint32_t object_colors_length;
00043       typedef moveit_msgs::ObjectColor _object_colors_type;
00044       _object_colors_type st_object_colors;
00045       _object_colors_type * object_colors;
00046       typedef moveit_msgs::PlanningSceneWorld _world_type;
00047       _world_type world;
00048       typedef bool _is_diff_type;
00049       _is_diff_type is_diff;
00050 
00051     PlanningScene():
00052       name(""),
00053       robot_state(),
00054       robot_model_name(""),
00055       fixed_frame_transforms_length(0), fixed_frame_transforms(NULL),
00056       allowed_collision_matrix(),
00057       link_padding_length(0), link_padding(NULL),
00058       link_scale_length(0), link_scale(NULL),
00059       object_colors_length(0), object_colors(NULL),
00060       world(),
00061       is_diff(0)
00062     {
00063     }
00064 
00065     virtual int serialize(unsigned char *outbuffer) const
00066     {
00067       int offset = 0;
00068       uint32_t length_name = strlen(this->name);
00069       varToArr(outbuffer + offset, length_name);
00070       offset += 4;
00071       memcpy(outbuffer + offset, this->name, length_name);
00072       offset += length_name;
00073       offset += this->robot_state.serialize(outbuffer + offset);
00074       uint32_t length_robot_model_name = strlen(this->robot_model_name);
00075       varToArr(outbuffer + offset, length_robot_model_name);
00076       offset += 4;
00077       memcpy(outbuffer + offset, this->robot_model_name, length_robot_model_name);
00078       offset += length_robot_model_name;
00079       *(outbuffer + offset + 0) = (this->fixed_frame_transforms_length >> (8 * 0)) & 0xFF;
00080       *(outbuffer + offset + 1) = (this->fixed_frame_transforms_length >> (8 * 1)) & 0xFF;
00081       *(outbuffer + offset + 2) = (this->fixed_frame_transforms_length >> (8 * 2)) & 0xFF;
00082       *(outbuffer + offset + 3) = (this->fixed_frame_transforms_length >> (8 * 3)) & 0xFF;
00083       offset += sizeof(this->fixed_frame_transforms_length);
00084       for( uint32_t i = 0; i < fixed_frame_transforms_length; i++){
00085       offset += this->fixed_frame_transforms[i].serialize(outbuffer + offset);
00086       }
00087       offset += this->allowed_collision_matrix.serialize(outbuffer + offset);
00088       *(outbuffer + offset + 0) = (this->link_padding_length >> (8 * 0)) & 0xFF;
00089       *(outbuffer + offset + 1) = (this->link_padding_length >> (8 * 1)) & 0xFF;
00090       *(outbuffer + offset + 2) = (this->link_padding_length >> (8 * 2)) & 0xFF;
00091       *(outbuffer + offset + 3) = (this->link_padding_length >> (8 * 3)) & 0xFF;
00092       offset += sizeof(this->link_padding_length);
00093       for( uint32_t i = 0; i < link_padding_length; i++){
00094       offset += this->link_padding[i].serialize(outbuffer + offset);
00095       }
00096       *(outbuffer + offset + 0) = (this->link_scale_length >> (8 * 0)) & 0xFF;
00097       *(outbuffer + offset + 1) = (this->link_scale_length >> (8 * 1)) & 0xFF;
00098       *(outbuffer + offset + 2) = (this->link_scale_length >> (8 * 2)) & 0xFF;
00099       *(outbuffer + offset + 3) = (this->link_scale_length >> (8 * 3)) & 0xFF;
00100       offset += sizeof(this->link_scale_length);
00101       for( uint32_t i = 0; i < link_scale_length; i++){
00102       offset += this->link_scale[i].serialize(outbuffer + offset);
00103       }
00104       *(outbuffer + offset + 0) = (this->object_colors_length >> (8 * 0)) & 0xFF;
00105       *(outbuffer + offset + 1) = (this->object_colors_length >> (8 * 1)) & 0xFF;
00106       *(outbuffer + offset + 2) = (this->object_colors_length >> (8 * 2)) & 0xFF;
00107       *(outbuffer + offset + 3) = (this->object_colors_length >> (8 * 3)) & 0xFF;
00108       offset += sizeof(this->object_colors_length);
00109       for( uint32_t i = 0; i < object_colors_length; i++){
00110       offset += this->object_colors[i].serialize(outbuffer + offset);
00111       }
00112       offset += this->world.serialize(outbuffer + offset);
00113       union {
00114         bool real;
00115         uint8_t base;
00116       } u_is_diff;
00117       u_is_diff.real = this->is_diff;
00118       *(outbuffer + offset + 0) = (u_is_diff.base >> (8 * 0)) & 0xFF;
00119       offset += sizeof(this->is_diff);
00120       return offset;
00121     }
00122 
00123     virtual int deserialize(unsigned char *inbuffer)
00124     {
00125       int offset = 0;
00126       uint32_t length_name;
00127       arrToVar(length_name, (inbuffer + offset));
00128       offset += 4;
00129       for(unsigned int k= offset; k< offset+length_name; ++k){
00130           inbuffer[k-1]=inbuffer[k];
00131       }
00132       inbuffer[offset+length_name-1]=0;
00133       this->name = (char *)(inbuffer + offset-1);
00134       offset += length_name;
00135       offset += this->robot_state.deserialize(inbuffer + offset);
00136       uint32_t length_robot_model_name;
00137       arrToVar(length_robot_model_name, (inbuffer + offset));
00138       offset += 4;
00139       for(unsigned int k= offset; k< offset+length_robot_model_name; ++k){
00140           inbuffer[k-1]=inbuffer[k];
00141       }
00142       inbuffer[offset+length_robot_model_name-1]=0;
00143       this->robot_model_name = (char *)(inbuffer + offset-1);
00144       offset += length_robot_model_name;
00145       uint32_t fixed_frame_transforms_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00146       fixed_frame_transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00147       fixed_frame_transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00148       fixed_frame_transforms_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00149       offset += sizeof(this->fixed_frame_transforms_length);
00150       if(fixed_frame_transforms_lengthT > fixed_frame_transforms_length)
00151         this->fixed_frame_transforms = (geometry_msgs::TransformStamped*)realloc(this->fixed_frame_transforms, fixed_frame_transforms_lengthT * sizeof(geometry_msgs::TransformStamped));
00152       fixed_frame_transforms_length = fixed_frame_transforms_lengthT;
00153       for( uint32_t i = 0; i < fixed_frame_transforms_length; i++){
00154       offset += this->st_fixed_frame_transforms.deserialize(inbuffer + offset);
00155         memcpy( &(this->fixed_frame_transforms[i]), &(this->st_fixed_frame_transforms), sizeof(geometry_msgs::TransformStamped));
00156       }
00157       offset += this->allowed_collision_matrix.deserialize(inbuffer + offset);
00158       uint32_t link_padding_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00159       link_padding_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00160       link_padding_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00161       link_padding_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00162       offset += sizeof(this->link_padding_length);
00163       if(link_padding_lengthT > link_padding_length)
00164         this->link_padding = (moveit_msgs::LinkPadding*)realloc(this->link_padding, link_padding_lengthT * sizeof(moveit_msgs::LinkPadding));
00165       link_padding_length = link_padding_lengthT;
00166       for( uint32_t i = 0; i < link_padding_length; i++){
00167       offset += this->st_link_padding.deserialize(inbuffer + offset);
00168         memcpy( &(this->link_padding[i]), &(this->st_link_padding), sizeof(moveit_msgs::LinkPadding));
00169       }
00170       uint32_t link_scale_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00171       link_scale_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00172       link_scale_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00173       link_scale_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00174       offset += sizeof(this->link_scale_length);
00175       if(link_scale_lengthT > link_scale_length)
00176         this->link_scale = (moveit_msgs::LinkScale*)realloc(this->link_scale, link_scale_lengthT * sizeof(moveit_msgs::LinkScale));
00177       link_scale_length = link_scale_lengthT;
00178       for( uint32_t i = 0; i < link_scale_length; i++){
00179       offset += this->st_link_scale.deserialize(inbuffer + offset);
00180         memcpy( &(this->link_scale[i]), &(this->st_link_scale), sizeof(moveit_msgs::LinkScale));
00181       }
00182       uint32_t object_colors_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00183       object_colors_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00184       object_colors_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00185       object_colors_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00186       offset += sizeof(this->object_colors_length);
00187       if(object_colors_lengthT > object_colors_length)
00188         this->object_colors = (moveit_msgs::ObjectColor*)realloc(this->object_colors, object_colors_lengthT * sizeof(moveit_msgs::ObjectColor));
00189       object_colors_length = object_colors_lengthT;
00190       for( uint32_t i = 0; i < object_colors_length; i++){
00191       offset += this->st_object_colors.deserialize(inbuffer + offset);
00192         memcpy( &(this->object_colors[i]), &(this->st_object_colors), sizeof(moveit_msgs::ObjectColor));
00193       }
00194       offset += this->world.deserialize(inbuffer + offset);
00195       union {
00196         bool real;
00197         uint8_t base;
00198       } u_is_diff;
00199       u_is_diff.base = 0;
00200       u_is_diff.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00201       this->is_diff = u_is_diff.real;
00202       offset += sizeof(this->is_diff);
00203      return offset;
00204     }
00205 
00206     virtual const char * getType(){ return "moveit_msgs/PlanningScene"; };
00207     virtual const char * getMD5(){ return "89aac6d20db967ba716cba5a86b1b9d5"; };
00208 
00209   };
00210 
00211 }
00212 #endif