GraspPlanning.h

00001 #ifndef _ROS_SERVICE_GraspPlanning_h
00002 #define _ROS_SERVICE_GraspPlanning_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "moveit_msgs/Grasp.h"
00008 #include "moveit_msgs/MoveItErrorCodes.h"
00009 #include "moveit_msgs/CollisionObject.h"
00010 
00011 namespace moveit_msgs
00012 {
00013 
00014 static const char GRASPPLANNING[] = "moveit_msgs/GraspPlanning";
00015 
00016   class GraspPlanningRequest : public ros::Msg
00017   {
00018     public:
00019       typedef const char* _group_name_type;
00020       _group_name_type group_name;
00021       typedef moveit_msgs::CollisionObject _target_type;
00022       _target_type target;
00023       uint32_t support_surfaces_length;
00024       typedef char* _support_surfaces_type;
00025       _support_surfaces_type st_support_surfaces;
00026       _support_surfaces_type * support_surfaces;
00027       uint32_t candidate_grasps_length;
00028       typedef moveit_msgs::Grasp _candidate_grasps_type;
00029       _candidate_grasps_type st_candidate_grasps;
00030       _candidate_grasps_type * candidate_grasps;
00031       uint32_t movable_obstacles_length;
00032       typedef moveit_msgs::CollisionObject _movable_obstacles_type;
00033       _movable_obstacles_type st_movable_obstacles;
00034       _movable_obstacles_type * movable_obstacles;
00035 
00036     GraspPlanningRequest():
00037       group_name(""),
00038       target(),
00039       support_surfaces_length(0), support_surfaces(NULL),
00040       candidate_grasps_length(0), candidate_grasps(NULL),
00041       movable_obstacles_length(0), movable_obstacles(NULL)
00042     {
00043     }
00044 
00045     virtual int serialize(unsigned char *outbuffer) const
00046     {
00047       int offset = 0;
00048       uint32_t length_group_name = strlen(this->group_name);
00049       varToArr(outbuffer + offset, length_group_name);
00050       offset += 4;
00051       memcpy(outbuffer + offset, this->group_name, length_group_name);
00052       offset += length_group_name;
00053       offset += this->target.serialize(outbuffer + offset);
00054       *(outbuffer + offset + 0) = (this->support_surfaces_length >> (8 * 0)) & 0xFF;
00055       *(outbuffer + offset + 1) = (this->support_surfaces_length >> (8 * 1)) & 0xFF;
00056       *(outbuffer + offset + 2) = (this->support_surfaces_length >> (8 * 2)) & 0xFF;
00057       *(outbuffer + offset + 3) = (this->support_surfaces_length >> (8 * 3)) & 0xFF;
00058       offset += sizeof(this->support_surfaces_length);
00059       for( uint32_t i = 0; i < support_surfaces_length; i++){
00060       uint32_t length_support_surfacesi = strlen(this->support_surfaces[i]);
00061       varToArr(outbuffer + offset, length_support_surfacesi);
00062       offset += 4;
00063       memcpy(outbuffer + offset, this->support_surfaces[i], length_support_surfacesi);
00064       offset += length_support_surfacesi;
00065       }
00066       *(outbuffer + offset + 0) = (this->candidate_grasps_length >> (8 * 0)) & 0xFF;
00067       *(outbuffer + offset + 1) = (this->candidate_grasps_length >> (8 * 1)) & 0xFF;
00068       *(outbuffer + offset + 2) = (this->candidate_grasps_length >> (8 * 2)) & 0xFF;
00069       *(outbuffer + offset + 3) = (this->candidate_grasps_length >> (8 * 3)) & 0xFF;
00070       offset += sizeof(this->candidate_grasps_length);
00071       for( uint32_t i = 0; i < candidate_grasps_length; i++){
00072       offset += this->candidate_grasps[i].serialize(outbuffer + offset);
00073       }
00074       *(outbuffer + offset + 0) = (this->movable_obstacles_length >> (8 * 0)) & 0xFF;
00075       *(outbuffer + offset + 1) = (this->movable_obstacles_length >> (8 * 1)) & 0xFF;
00076       *(outbuffer + offset + 2) = (this->movable_obstacles_length >> (8 * 2)) & 0xFF;
00077       *(outbuffer + offset + 3) = (this->movable_obstacles_length >> (8 * 3)) & 0xFF;
00078       offset += sizeof(this->movable_obstacles_length);
00079       for( uint32_t i = 0; i < movable_obstacles_length; i++){
00080       offset += this->movable_obstacles[i].serialize(outbuffer + offset);
00081       }
00082       return offset;
00083     }
00084 
00085     virtual int deserialize(unsigned char *inbuffer)
00086     {
00087       int offset = 0;
00088       uint32_t length_group_name;
00089       arrToVar(length_group_name, (inbuffer + offset));
00090       offset += 4;
00091       for(unsigned int k= offset; k< offset+length_group_name; ++k){
00092           inbuffer[k-1]=inbuffer[k];
00093       }
00094       inbuffer[offset+length_group_name-1]=0;
00095       this->group_name = (char *)(inbuffer + offset-1);
00096       offset += length_group_name;
00097       offset += this->target.deserialize(inbuffer + offset);
00098       uint32_t support_surfaces_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00099       support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00100       support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00101       support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00102       offset += sizeof(this->support_surfaces_length);
00103       if(support_surfaces_lengthT > support_surfaces_length)
00104         this->support_surfaces = (char**)realloc(this->support_surfaces, support_surfaces_lengthT * sizeof(char*));
00105       support_surfaces_length = support_surfaces_lengthT;
00106       for( uint32_t i = 0; i < support_surfaces_length; i++){
00107       uint32_t length_st_support_surfaces;
00108       arrToVar(length_st_support_surfaces, (inbuffer + offset));
00109       offset += 4;
00110       for(unsigned int k= offset; k< offset+length_st_support_surfaces; ++k){
00111           inbuffer[k-1]=inbuffer[k];
00112       }
00113       inbuffer[offset+length_st_support_surfaces-1]=0;
00114       this->st_support_surfaces = (char *)(inbuffer + offset-1);
00115       offset += length_st_support_surfaces;
00116         memcpy( &(this->support_surfaces[i]), &(this->st_support_surfaces), sizeof(char*));
00117       }
00118       uint32_t candidate_grasps_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00119       candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00120       candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00121       candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00122       offset += sizeof(this->candidate_grasps_length);
00123       if(candidate_grasps_lengthT > candidate_grasps_length)
00124         this->candidate_grasps = (moveit_msgs::Grasp*)realloc(this->candidate_grasps, candidate_grasps_lengthT * sizeof(moveit_msgs::Grasp));
00125       candidate_grasps_length = candidate_grasps_lengthT;
00126       for( uint32_t i = 0; i < candidate_grasps_length; i++){
00127       offset += this->st_candidate_grasps.deserialize(inbuffer + offset);
00128         memcpy( &(this->candidate_grasps[i]), &(this->st_candidate_grasps), sizeof(moveit_msgs::Grasp));
00129       }
00130       uint32_t movable_obstacles_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00131       movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00132       movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00133       movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00134       offset += sizeof(this->movable_obstacles_length);
00135       if(movable_obstacles_lengthT > movable_obstacles_length)
00136         this->movable_obstacles = (moveit_msgs::CollisionObject*)realloc(this->movable_obstacles, movable_obstacles_lengthT * sizeof(moveit_msgs::CollisionObject));
00137       movable_obstacles_length = movable_obstacles_lengthT;
00138       for( uint32_t i = 0; i < movable_obstacles_length; i++){
00139       offset += this->st_movable_obstacles.deserialize(inbuffer + offset);
00140         memcpy( &(this->movable_obstacles[i]), &(this->st_movable_obstacles), sizeof(moveit_msgs::CollisionObject));
00141       }
00142      return offset;
00143     }
00144 
00145     virtual const char * getType(){ return GRASPPLANNING; };
00146     virtual const char * getMD5(){ return "c234e9a645708cc86b57a43999746ae6"; };
00147 
00148   };
00149 
00150   class GraspPlanningResponse : public ros::Msg
00151   {
00152     public:
00153       uint32_t grasps_length;
00154       typedef moveit_msgs::Grasp _grasps_type;
00155       _grasps_type st_grasps;
00156       _grasps_type * grasps;
00157       typedef moveit_msgs::MoveItErrorCodes _error_code_type;
00158       _error_code_type error_code;
00159 
00160     GraspPlanningResponse():
00161       grasps_length(0), grasps(NULL),
00162       error_code()
00163     {
00164     }
00165 
00166     virtual int serialize(unsigned char *outbuffer) const
00167     {
00168       int offset = 0;
00169       *(outbuffer + offset + 0) = (this->grasps_length >> (8 * 0)) & 0xFF;
00170       *(outbuffer + offset + 1) = (this->grasps_length >> (8 * 1)) & 0xFF;
00171       *(outbuffer + offset + 2) = (this->grasps_length >> (8 * 2)) & 0xFF;
00172       *(outbuffer + offset + 3) = (this->grasps_length >> (8 * 3)) & 0xFF;
00173       offset += sizeof(this->grasps_length);
00174       for( uint32_t i = 0; i < grasps_length; i++){
00175       offset += this->grasps[i].serialize(outbuffer + offset);
00176       }
00177       offset += this->error_code.serialize(outbuffer + offset);
00178       return offset;
00179     }
00180 
00181     virtual int deserialize(unsigned char *inbuffer)
00182     {
00183       int offset = 0;
00184       uint32_t grasps_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00185       grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00186       grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00187       grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00188       offset += sizeof(this->grasps_length);
00189       if(grasps_lengthT > grasps_length)
00190         this->grasps = (moveit_msgs::Grasp*)realloc(this->grasps, grasps_lengthT * sizeof(moveit_msgs::Grasp));
00191       grasps_length = grasps_lengthT;
00192       for( uint32_t i = 0; i < grasps_length; i++){
00193       offset += this->st_grasps.deserialize(inbuffer + offset);
00194         memcpy( &(this->grasps[i]), &(this->st_grasps), sizeof(moveit_msgs::Grasp));
00195       }
00196       offset += this->error_code.deserialize(inbuffer + offset);
00197      return offset;
00198     }
00199 
00200     virtual const char * getType(){ return GRASPPLANNING; };
00201     virtual const char * getMD5(){ return "bb8169d403b6e9f96bf61e22a50d13ae"; };
00202 
00203   };
00204 
00205   class GraspPlanning {
00206     public:
00207     typedef GraspPlanningRequest Request;
00208     typedef GraspPlanningResponse Response;
00209   };
00210 
00211 }
00212 #endif