Working towards recieving twists

Dependencies:   BufferedSerial

Fork of ros_lib_kinetic by Gary Servin

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers GetModelState.h Source File

GetModelState.h

00001 #ifndef _ROS_SERVICE_GetModelState_h
00002 #define _ROS_SERVICE_GetModelState_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "geometry_msgs/Pose.h"
00008 #include "geometry_msgs/Twist.h"
00009 #include "std_msgs/Header.h"
00010 
00011 namespace gazebo_msgs
00012 {
00013 
00014 static const char GETMODELSTATE[] = "gazebo_msgs/GetModelState";
00015 
00016   class GetModelStateRequest : public ros::Msg
00017   {
00018     public:
00019       typedef const char* _model_name_type;
00020       _model_name_type model_name;
00021       typedef const char* _relative_entity_name_type;
00022       _relative_entity_name_type relative_entity_name;
00023 
00024     GetModelStateRequest():
00025       model_name(""),
00026       relative_entity_name("")
00027     {
00028     }
00029 
00030     virtual int serialize(unsigned char *outbuffer) const
00031     {
00032       int offset = 0;
00033       uint32_t length_model_name = strlen(this->model_name);
00034       varToArr(outbuffer + offset, length_model_name);
00035       offset += 4;
00036       memcpy(outbuffer + offset, this->model_name, length_model_name);
00037       offset += length_model_name;
00038       uint32_t length_relative_entity_name = strlen(this->relative_entity_name);
00039       varToArr(outbuffer + offset, length_relative_entity_name);
00040       offset += 4;
00041       memcpy(outbuffer + offset, this->relative_entity_name, length_relative_entity_name);
00042       offset += length_relative_entity_name;
00043       return offset;
00044     }
00045 
00046     virtual int deserialize(unsigned char *inbuffer)
00047     {
00048       int offset = 0;
00049       uint32_t length_model_name;
00050       arrToVar(length_model_name, (inbuffer + offset));
00051       offset += 4;
00052       for(unsigned int k= offset; k< offset+length_model_name; ++k){
00053           inbuffer[k-1]=inbuffer[k];
00054       }
00055       inbuffer[offset+length_model_name-1]=0;
00056       this->model_name = (char *)(inbuffer + offset-1);
00057       offset += length_model_name;
00058       uint32_t length_relative_entity_name;
00059       arrToVar(length_relative_entity_name, (inbuffer + offset));
00060       offset += 4;
00061       for(unsigned int k= offset; k< offset+length_relative_entity_name; ++k){
00062           inbuffer[k-1]=inbuffer[k];
00063       }
00064       inbuffer[offset+length_relative_entity_name-1]=0;
00065       this->relative_entity_name = (char *)(inbuffer + offset-1);
00066       offset += length_relative_entity_name;
00067      return offset;
00068     }
00069 
00070     const char * getType(){ return GETMODELSTATE; };
00071     const char * getMD5(){ return "19d412713cefe4a67437e17a951e759e"; };
00072 
00073   };
00074 
00075   class GetModelStateResponse : public ros::Msg
00076   {
00077     public:
00078       typedef std_msgs::Header _header_type;
00079       _header_type header;
00080       typedef geometry_msgs::Pose _pose_type;
00081       _pose_type pose;
00082       typedef geometry_msgs::Twist _twist_type;
00083       _twist_type twist;
00084       typedef bool _success_type;
00085       _success_type success;
00086       typedef const char* _status_message_type;
00087       _status_message_type status_message;
00088 
00089     GetModelStateResponse():
00090       header(),
00091       pose(),
00092       twist(),
00093       success(0),
00094       status_message("")
00095     {
00096     }
00097 
00098     virtual int serialize(unsigned char *outbuffer) const
00099     {
00100       int offset = 0;
00101       offset += this->header.serialize(outbuffer + offset);
00102       offset += this->pose.serialize(outbuffer + offset);
00103       offset += this->twist.serialize(outbuffer + offset);
00104       union {
00105         bool real;
00106         uint8_t base;
00107       } u_success;
00108       u_success.real = this->success;
00109       *(outbuffer + offset + 0) = (u_success.base >> (8 * 0)) & 0xFF;
00110       offset += sizeof(this->success);
00111       uint32_t length_status_message = strlen(this->status_message);
00112       varToArr(outbuffer + offset, length_status_message);
00113       offset += 4;
00114       memcpy(outbuffer + offset, this->status_message, length_status_message);
00115       offset += length_status_message;
00116       return offset;
00117     }
00118 
00119     virtual int deserialize(unsigned char *inbuffer)
00120     {
00121       int offset = 0;
00122       offset += this->header.deserialize(inbuffer + offset);
00123       offset += this->pose.deserialize(inbuffer + offset);
00124       offset += this->twist.deserialize(inbuffer + offset);
00125       union {
00126         bool real;
00127         uint8_t base;
00128       } u_success;
00129       u_success.base = 0;
00130       u_success.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00131       this->success = u_success.real;
00132       offset += sizeof(this->success);
00133       uint32_t length_status_message;
00134       arrToVar(length_status_message, (inbuffer + offset));
00135       offset += 4;
00136       for(unsigned int k= offset; k< offset+length_status_message; ++k){
00137           inbuffer[k-1]=inbuffer[k];
00138       }
00139       inbuffer[offset+length_status_message-1]=0;
00140       this->status_message = (char *)(inbuffer + offset-1);
00141       offset += length_status_message;
00142      return offset;
00143     }
00144 
00145     const char * getType(){ return GETMODELSTATE; };
00146     const char * getMD5(){ return "ccd51739bb00f0141629e87b792e92b9"; };
00147 
00148   };
00149 
00150   class GetModelState {
00151     public:
00152     typedef GetModelStateRequest Request;
00153     typedef GetModelStateResponse Response;
00154   };
00155 
00156 }
00157 #endif