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 SetJointTrajectory.h Source File

SetJointTrajectory.h

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