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