GetCartesianPath.h

00001 #ifndef _ROS_SERVICE_GetCartesianPath_h
00002 #define _ROS_SERVICE_GetCartesianPath_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "std_msgs/Header.h"
00008 #include "moveit_msgs/RobotTrajectory.h"
00009 #include "moveit_msgs/RobotState.h"
00010 #include "moveit_msgs/MoveItErrorCodes.h"
00011 #include "geometry_msgs/Pose.h"
00012 #include "moveit_msgs/Constraints.h"
00013 
00014 namespace moveit_msgs
00015 {
00016 
00017 static const char GETCARTESIANPATH[] = "moveit_msgs/GetCartesianPath";
00018 
00019   class GetCartesianPathRequest : public ros::Msg
00020   {
00021     public:
00022       typedef std_msgs::Header _header_type;
00023       _header_type header;
00024       typedef moveit_msgs::RobotState _start_state_type;
00025       _start_state_type start_state;
00026       typedef const char* _group_name_type;
00027       _group_name_type group_name;
00028       typedef const char* _link_name_type;
00029       _link_name_type link_name;
00030       uint32_t waypoints_length;
00031       typedef geometry_msgs::Pose _waypoints_type;
00032       _waypoints_type st_waypoints;
00033       _waypoints_type * waypoints;
00034       typedef double _max_step_type;
00035       _max_step_type max_step;
00036       typedef double _jump_threshold_type;
00037       _jump_threshold_type jump_threshold;
00038       typedef bool _avoid_collisions_type;
00039       _avoid_collisions_type avoid_collisions;
00040       typedef moveit_msgs::Constraints _path_constraints_type;
00041       _path_constraints_type path_constraints;
00042 
00043     GetCartesianPathRequest():
00044       header(),
00045       start_state(),
00046       group_name(""),
00047       link_name(""),
00048       waypoints_length(0), waypoints(NULL),
00049       max_step(0),
00050       jump_threshold(0),
00051       avoid_collisions(0),
00052       path_constraints()
00053     {
00054     }
00055 
00056     virtual int serialize(unsigned char *outbuffer) const
00057     {
00058       int offset = 0;
00059       offset += this->header.serialize(outbuffer + offset);
00060       offset += this->start_state.serialize(outbuffer + offset);
00061       uint32_t length_group_name = strlen(this->group_name);
00062       varToArr(outbuffer + offset, length_group_name);
00063       offset += 4;
00064       memcpy(outbuffer + offset, this->group_name, length_group_name);
00065       offset += length_group_name;
00066       uint32_t length_link_name = strlen(this->link_name);
00067       varToArr(outbuffer + offset, length_link_name);
00068       offset += 4;
00069       memcpy(outbuffer + offset, this->link_name, length_link_name);
00070       offset += length_link_name;
00071       *(outbuffer + offset + 0) = (this->waypoints_length >> (8 * 0)) & 0xFF;
00072       *(outbuffer + offset + 1) = (this->waypoints_length >> (8 * 1)) & 0xFF;
00073       *(outbuffer + offset + 2) = (this->waypoints_length >> (8 * 2)) & 0xFF;
00074       *(outbuffer + offset + 3) = (this->waypoints_length >> (8 * 3)) & 0xFF;
00075       offset += sizeof(this->waypoints_length);
00076       for( uint32_t i = 0; i < waypoints_length; i++){
00077       offset += this->waypoints[i].serialize(outbuffer + offset);
00078       }
00079       union {
00080         double real;
00081         uint64_t base;
00082       } u_max_step;
00083       u_max_step.real = this->max_step;
00084       *(outbuffer + offset + 0) = (u_max_step.base >> (8 * 0)) & 0xFF;
00085       *(outbuffer + offset + 1) = (u_max_step.base >> (8 * 1)) & 0xFF;
00086       *(outbuffer + offset + 2) = (u_max_step.base >> (8 * 2)) & 0xFF;
00087       *(outbuffer + offset + 3) = (u_max_step.base >> (8 * 3)) & 0xFF;
00088       *(outbuffer + offset + 4) = (u_max_step.base >> (8 * 4)) & 0xFF;
00089       *(outbuffer + offset + 5) = (u_max_step.base >> (8 * 5)) & 0xFF;
00090       *(outbuffer + offset + 6) = (u_max_step.base >> (8 * 6)) & 0xFF;
00091       *(outbuffer + offset + 7) = (u_max_step.base >> (8 * 7)) & 0xFF;
00092       offset += sizeof(this->max_step);
00093       union {
00094         double real;
00095         uint64_t base;
00096       } u_jump_threshold;
00097       u_jump_threshold.real = this->jump_threshold;
00098       *(outbuffer + offset + 0) = (u_jump_threshold.base >> (8 * 0)) & 0xFF;
00099       *(outbuffer + offset + 1) = (u_jump_threshold.base >> (8 * 1)) & 0xFF;
00100       *(outbuffer + offset + 2) = (u_jump_threshold.base >> (8 * 2)) & 0xFF;
00101       *(outbuffer + offset + 3) = (u_jump_threshold.base >> (8 * 3)) & 0xFF;
00102       *(outbuffer + offset + 4) = (u_jump_threshold.base >> (8 * 4)) & 0xFF;
00103       *(outbuffer + offset + 5) = (u_jump_threshold.base >> (8 * 5)) & 0xFF;
00104       *(outbuffer + offset + 6) = (u_jump_threshold.base >> (8 * 6)) & 0xFF;
00105       *(outbuffer + offset + 7) = (u_jump_threshold.base >> (8 * 7)) & 0xFF;
00106       offset += sizeof(this->jump_threshold);
00107       union {
00108         bool real;
00109         uint8_t base;
00110       } u_avoid_collisions;
00111       u_avoid_collisions.real = this->avoid_collisions;
00112       *(outbuffer + offset + 0) = (u_avoid_collisions.base >> (8 * 0)) & 0xFF;
00113       offset += sizeof(this->avoid_collisions);
00114       offset += this->path_constraints.serialize(outbuffer + offset);
00115       return offset;
00116     }
00117 
00118     virtual int deserialize(unsigned char *inbuffer)
00119     {
00120       int offset = 0;
00121       offset += this->header.deserialize(inbuffer + offset);
00122       offset += this->start_state.deserialize(inbuffer + offset);
00123       uint32_t length_group_name;
00124       arrToVar(length_group_name, (inbuffer + offset));
00125       offset += 4;
00126       for(unsigned int k= offset; k< offset+length_group_name; ++k){
00127           inbuffer[k-1]=inbuffer[k];
00128       }
00129       inbuffer[offset+length_group_name-1]=0;
00130       this->group_name = (char *)(inbuffer + offset-1);
00131       offset += length_group_name;
00132       uint32_t length_link_name;
00133       arrToVar(length_link_name, (inbuffer + offset));
00134       offset += 4;
00135       for(unsigned int k= offset; k< offset+length_link_name; ++k){
00136           inbuffer[k-1]=inbuffer[k];
00137       }
00138       inbuffer[offset+length_link_name-1]=0;
00139       this->link_name = (char *)(inbuffer + offset-1);
00140       offset += length_link_name;
00141       uint32_t waypoints_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00142       waypoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00143       waypoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00144       waypoints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00145       offset += sizeof(this->waypoints_length);
00146       if(waypoints_lengthT > waypoints_length)
00147         this->waypoints = (geometry_msgs::Pose*)realloc(this->waypoints, waypoints_lengthT * sizeof(geometry_msgs::Pose));
00148       waypoints_length = waypoints_lengthT;
00149       for( uint32_t i = 0; i < waypoints_length; i++){
00150       offset += this->st_waypoints.deserialize(inbuffer + offset);
00151         memcpy( &(this->waypoints[i]), &(this->st_waypoints), sizeof(geometry_msgs::Pose));
00152       }
00153       union {
00154         double real;
00155         uint64_t base;
00156       } u_max_step;
00157       u_max_step.base = 0;
00158       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00159       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00160       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00161       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00162       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00163       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00164       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00165       u_max_step.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00166       this->max_step = u_max_step.real;
00167       offset += sizeof(this->max_step);
00168       union {
00169         double real;
00170         uint64_t base;
00171       } u_jump_threshold;
00172       u_jump_threshold.base = 0;
00173       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00174       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00175       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00176       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00177       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00178       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00179       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00180       u_jump_threshold.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00181       this->jump_threshold = u_jump_threshold.real;
00182       offset += sizeof(this->jump_threshold);
00183       union {
00184         bool real;
00185         uint8_t base;
00186       } u_avoid_collisions;
00187       u_avoid_collisions.base = 0;
00188       u_avoid_collisions.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00189       this->avoid_collisions = u_avoid_collisions.real;
00190       offset += sizeof(this->avoid_collisions);
00191       offset += this->path_constraints.deserialize(inbuffer + offset);
00192      return offset;
00193     }
00194 
00195     virtual const char * getType(){ return GETCARTESIANPATH; };
00196     virtual const char * getMD5(){ return "b37c16ad7ed838d811a270a8054276b6"; };
00197 
00198   };
00199 
00200   class GetCartesianPathResponse : public ros::Msg
00201   {
00202     public:
00203       typedef moveit_msgs::RobotState _start_state_type;
00204       _start_state_type start_state;
00205       typedef moveit_msgs::RobotTrajectory _solution_type;
00206       _solution_type solution;
00207       typedef double _fraction_type;
00208       _fraction_type fraction;
00209       typedef moveit_msgs::MoveItErrorCodes _error_code_type;
00210       _error_code_type error_code;
00211 
00212     GetCartesianPathResponse():
00213       start_state(),
00214       solution(),
00215       fraction(0),
00216       error_code()
00217     {
00218     }
00219 
00220     virtual int serialize(unsigned char *outbuffer) const
00221     {
00222       int offset = 0;
00223       offset += this->start_state.serialize(outbuffer + offset);
00224       offset += this->solution.serialize(outbuffer + offset);
00225       union {
00226         double real;
00227         uint64_t base;
00228       } u_fraction;
00229       u_fraction.real = this->fraction;
00230       *(outbuffer + offset + 0) = (u_fraction.base >> (8 * 0)) & 0xFF;
00231       *(outbuffer + offset + 1) = (u_fraction.base >> (8 * 1)) & 0xFF;
00232       *(outbuffer + offset + 2) = (u_fraction.base >> (8 * 2)) & 0xFF;
00233       *(outbuffer + offset + 3) = (u_fraction.base >> (8 * 3)) & 0xFF;
00234       *(outbuffer + offset + 4) = (u_fraction.base >> (8 * 4)) & 0xFF;
00235       *(outbuffer + offset + 5) = (u_fraction.base >> (8 * 5)) & 0xFF;
00236       *(outbuffer + offset + 6) = (u_fraction.base >> (8 * 6)) & 0xFF;
00237       *(outbuffer + offset + 7) = (u_fraction.base >> (8 * 7)) & 0xFF;
00238       offset += sizeof(this->fraction);
00239       offset += this->error_code.serialize(outbuffer + offset);
00240       return offset;
00241     }
00242 
00243     virtual int deserialize(unsigned char *inbuffer)
00244     {
00245       int offset = 0;
00246       offset += this->start_state.deserialize(inbuffer + offset);
00247       offset += this->solution.deserialize(inbuffer + offset);
00248       union {
00249         double real;
00250         uint64_t base;
00251       } u_fraction;
00252       u_fraction.base = 0;
00253       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00254       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00255       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00256       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00257       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00258       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00259       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00260       u_fraction.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00261       this->fraction = u_fraction.real;
00262       offset += sizeof(this->fraction);
00263       offset += this->error_code.deserialize(inbuffer + offset);
00264      return offset;
00265     }
00266 
00267     virtual const char * getType(){ return GETCARTESIANPATH; };
00268     virtual const char * getMD5(){ return "45414110461a45eb0e273e013924ce59"; };
00269 
00270   };
00271 
00272   class GetCartesianPath {
00273     public:
00274     typedef GetCartesianPathRequest Request;
00275     typedef GetCartesianPathResponse Response;
00276   };
00277 
00278 }
00279 #endif