GetStateValidity.h
Go to the documentation of this file.
00001 #ifndef _ROS_SERVICE_GetStateValidity_h
00002 #define _ROS_SERVICE_GetStateValidity_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "moveit_msgs/ConstraintEvalResult.h"
00008 #include "moveit_msgs/CostSource.h"
00009 #include "moveit_msgs/ContactInformation.h"
00010 #include "moveit_msgs/RobotState.h"
00011 #include "moveit_msgs/Constraints.h"
00012 
00013 namespace moveit_msgs
00014 {
00015 
00016 static const char GETSTATEVALIDITY[] = "moveit_msgs/GetStateValidity";
00017 
00018   class GetStateValidityRequest : public ros::Msg
00019   {
00020     public:
00021       moveit_msgs::RobotState robot_state;
00022       const char* group_name;
00023       moveit_msgs::Constraints constraints;
00024 
00025     virtual int serialize(unsigned char *outbuffer) const
00026     {
00027       int offset = 0;
00028       offset += this->robot_state.serialize(outbuffer + offset);
00029       uint32_t length_group_name = strlen(this->group_name);
00030       memcpy(outbuffer + offset, &length_group_name, sizeof(uint32_t));
00031       offset += 4;
00032       memcpy(outbuffer + offset, this->group_name, length_group_name);
00033       offset += length_group_name;
00034       offset += this->constraints.serialize(outbuffer + offset);
00035       return offset;
00036     }
00037 
00038     virtual int deserialize(unsigned char *inbuffer)
00039     {
00040       int offset = 0;
00041       offset += this->robot_state.deserialize(inbuffer + offset);
00042       uint32_t length_group_name;
00043       memcpy(&length_group_name, (inbuffer + offset), sizeof(uint32_t));
00044       offset += 4;
00045       for(unsigned int k= offset; k< offset+length_group_name; ++k){
00046           inbuffer[k-1]=inbuffer[k];
00047       }
00048       inbuffer[offset+length_group_name-1]=0;
00049       this->group_name = (char *)(inbuffer + offset-1);
00050       offset += length_group_name;
00051       offset += this->constraints.deserialize(inbuffer + offset);
00052      return offset;
00053     }
00054 
00055     const char * getType(){ return GETSTATEVALIDITY; };
00056     const char * getMD5(){ return "b569c609cafad20ba7d0e46e70e7cab1"; };
00057 
00058   };
00059 
00060   class GetStateValidityResponse : public ros::Msg
00061   {
00062     public:
00063       bool valid;
00064       uint8_t contacts_length;
00065       moveit_msgs::ContactInformation st_contacts;
00066       moveit_msgs::ContactInformation * contacts;
00067       uint8_t cost_sources_length;
00068       moveit_msgs::CostSource st_cost_sources;
00069       moveit_msgs::CostSource * cost_sources;
00070       uint8_t constraint_result_length;
00071       moveit_msgs::ConstraintEvalResult st_constraint_result;
00072       moveit_msgs::ConstraintEvalResult * constraint_result;
00073 
00074     virtual int serialize(unsigned char *outbuffer) const
00075     {
00076       int offset = 0;
00077       union {
00078         bool real;
00079         uint8_t base;
00080       } u_valid;
00081       u_valid.real = this->valid;
00082       *(outbuffer + offset + 0) = (u_valid.base >> (8 * 0)) & 0xFF;
00083       offset += sizeof(this->valid);
00084       *(outbuffer + offset++) = contacts_length;
00085       *(outbuffer + offset++) = 0;
00086       *(outbuffer + offset++) = 0;
00087       *(outbuffer + offset++) = 0;
00088       for( uint8_t i = 0; i < contacts_length; i++){
00089       offset += this->contacts[i].serialize(outbuffer + offset);
00090       }
00091       *(outbuffer + offset++) = cost_sources_length;
00092       *(outbuffer + offset++) = 0;
00093       *(outbuffer + offset++) = 0;
00094       *(outbuffer + offset++) = 0;
00095       for( uint8_t i = 0; i < cost_sources_length; i++){
00096       offset += this->cost_sources[i].serialize(outbuffer + offset);
00097       }
00098       *(outbuffer + offset++) = constraint_result_length;
00099       *(outbuffer + offset++) = 0;
00100       *(outbuffer + offset++) = 0;
00101       *(outbuffer + offset++) = 0;
00102       for( uint8_t i = 0; i < constraint_result_length; i++){
00103       offset += this->constraint_result[i].serialize(outbuffer + offset);
00104       }
00105       return offset;
00106     }
00107 
00108     virtual int deserialize(unsigned char *inbuffer)
00109     {
00110       int offset = 0;
00111       union {
00112         bool real;
00113         uint8_t base;
00114       } u_valid;
00115       u_valid.base = 0;
00116       u_valid.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00117       this->valid = u_valid.real;
00118       offset += sizeof(this->valid);
00119       uint8_t contacts_lengthT = *(inbuffer + offset++);
00120       if(contacts_lengthT > contacts_length)
00121         this->contacts = (moveit_msgs::ContactInformation*)realloc(this->contacts, contacts_lengthT * sizeof(moveit_msgs::ContactInformation));
00122       offset += 3;
00123       contacts_length = contacts_lengthT;
00124       for( uint8_t i = 0; i < contacts_length; i++){
00125       offset += this->st_contacts.deserialize(inbuffer + offset);
00126         memcpy( &(this->contacts[i]), &(this->st_contacts), sizeof(moveit_msgs::ContactInformation));
00127       }
00128       uint8_t cost_sources_lengthT = *(inbuffer + offset++);
00129       if(cost_sources_lengthT > cost_sources_length)
00130         this->cost_sources = (moveit_msgs::CostSource*)realloc(this->cost_sources, cost_sources_lengthT * sizeof(moveit_msgs::CostSource));
00131       offset += 3;
00132       cost_sources_length = cost_sources_lengthT;
00133       for( uint8_t i = 0; i < cost_sources_length; i++){
00134       offset += this->st_cost_sources.deserialize(inbuffer + offset);
00135         memcpy( &(this->cost_sources[i]), &(this->st_cost_sources), sizeof(moveit_msgs::CostSource));
00136       }
00137       uint8_t constraint_result_lengthT = *(inbuffer + offset++);
00138       if(constraint_result_lengthT > constraint_result_length)
00139         this->constraint_result = (moveit_msgs::ConstraintEvalResult*)realloc(this->constraint_result, constraint_result_lengthT * sizeof(moveit_msgs::ConstraintEvalResult));
00140       offset += 3;
00141       constraint_result_length = constraint_result_lengthT;
00142       for( uint8_t i = 0; i < constraint_result_length; i++){
00143       offset += this->st_constraint_result.deserialize(inbuffer + offset);
00144         memcpy( &(this->constraint_result[i]), &(this->st_constraint_result), sizeof(moveit_msgs::ConstraintEvalResult));
00145       }
00146      return offset;
00147     }
00148 
00149     const char * getType(){ return GETSTATEVALIDITY; };
00150     const char * getMD5(){ return "e326fb22b7448f29c0e726d9270d9929"; };
00151 
00152   };
00153 
00154   class GetStateValidity {
00155     public:
00156     typedef GetStateValidityRequest Request;
00157     typedef GetStateValidityResponse Response;
00158   };
00159 
00160 }
00161 #endif


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:55