ROS Serial library for Mbed platforms for ROS Indigo Igloo. Check http://wiki.ros.org/rosserial_mbed/ for more information

Dependencies:   BufferedSerial

Dependents:   rosserial_mbed_hello_world_publisher rtos_base_control rosserial_mbed_F64MA ROS-RTOS ... more

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