catchrobo2022 / Mbed 2 deprecated catchrobo2022_mbed

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers SetPlannerParams.h Source File

SetPlannerParams.h

00001 #ifndef _ROS_SERVICE_SetPlannerParams_h
00002 #define _ROS_SERVICE_SetPlannerParams_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "moveit_msgs/PlannerParams.h"
00008 
00009 namespace moveit_msgs
00010 {
00011 
00012 static const char SETPLANNERPARAMS[] = "moveit_msgs/SetPlannerParams";
00013 
00014   class SetPlannerParamsRequest : public ros::Msg
00015   {
00016     public:
00017       typedef const char* _planner_config_type;
00018       _planner_config_type planner_config;
00019       typedef const char* _group_type;
00020       _group_type group;
00021       typedef moveit_msgs::PlannerParams _params_type;
00022       _params_type params;
00023       typedef bool _replace_type;
00024       _replace_type replace;
00025 
00026     SetPlannerParamsRequest():
00027       planner_config(""),
00028       group(""),
00029       params(),
00030       replace(0)
00031     {
00032     }
00033 
00034     virtual int serialize(unsigned char *outbuffer) const
00035     {
00036       int offset = 0;
00037       uint32_t length_planner_config = strlen(this->planner_config);
00038       varToArr(outbuffer + offset, length_planner_config);
00039       offset += 4;
00040       memcpy(outbuffer + offset, this->planner_config, length_planner_config);
00041       offset += length_planner_config;
00042       uint32_t length_group = strlen(this->group);
00043       varToArr(outbuffer + offset, length_group);
00044       offset += 4;
00045       memcpy(outbuffer + offset, this->group, length_group);
00046       offset += length_group;
00047       offset += this->params.serialize(outbuffer + offset);
00048       union {
00049         bool real;
00050         uint8_t base;
00051       } u_replace;
00052       u_replace.real = this->replace;
00053       *(outbuffer + offset + 0) = (u_replace.base >> (8 * 0)) & 0xFF;
00054       offset += sizeof(this->replace);
00055       return offset;
00056     }
00057 
00058     virtual int deserialize(unsigned char *inbuffer)
00059     {
00060       int offset = 0;
00061       uint32_t length_planner_config;
00062       arrToVar(length_planner_config, (inbuffer + offset));
00063       offset += 4;
00064       for(unsigned int k= offset; k< offset+length_planner_config; ++k){
00065           inbuffer[k-1]=inbuffer[k];
00066       }
00067       inbuffer[offset+length_planner_config-1]=0;
00068       this->planner_config = (char *)(inbuffer + offset-1);
00069       offset += length_planner_config;
00070       uint32_t length_group;
00071       arrToVar(length_group, (inbuffer + offset));
00072       offset += 4;
00073       for(unsigned int k= offset; k< offset+length_group; ++k){
00074           inbuffer[k-1]=inbuffer[k];
00075       }
00076       inbuffer[offset+length_group-1]=0;
00077       this->group = (char *)(inbuffer + offset-1);
00078       offset += length_group;
00079       offset += this->params.deserialize(inbuffer + offset);
00080       union {
00081         bool real;
00082         uint8_t base;
00083       } u_replace;
00084       u_replace.base = 0;
00085       u_replace.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00086       this->replace = u_replace.real;
00087       offset += sizeof(this->replace);
00088      return offset;
00089     }
00090 
00091     virtual const char * getType(){ return SETPLANNERPARAMS; };
00092     virtual const char * getMD5(){ return "86762d89189c5f52cda7680fdbceb1db"; };
00093 
00094   };
00095 
00096   class SetPlannerParamsResponse : public ros::Msg
00097   {
00098     public:
00099 
00100     SetPlannerParamsResponse()
00101     {
00102     }
00103 
00104     virtual int serialize(unsigned char *outbuffer) const
00105     {
00106       int offset = 0;
00107       return offset;
00108     }
00109 
00110     virtual int deserialize(unsigned char *inbuffer)
00111     {
00112       int offset = 0;
00113      return offset;
00114     }
00115 
00116     virtual const char * getType(){ return SETPLANNERPARAMS; };
00117     virtual const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; };
00118 
00119   };
00120 
00121   class SetPlannerParams {
00122     public:
00123     typedef SetPlannerParamsRequest Request;
00124     typedef SetPlannerParamsResponse Response;
00125   };
00126 
00127 }
00128 #endif