DisplayTrajectory.h

00001 #ifndef _ROS_moveit_msgs_DisplayTrajectory_h
00002 #define _ROS_moveit_msgs_DisplayTrajectory_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "moveit_msgs/RobotTrajectory.h"
00009 #include "moveit_msgs/RobotState.h"
00010 
00011 namespace moveit_msgs
00012 {
00013 
00014   class DisplayTrajectory : public ros::Msg
00015   {
00016     public:
00017       typedef const char* _model_id_type;
00018       _model_id_type model_id;
00019       uint32_t trajectory_length;
00020       typedef moveit_msgs::RobotTrajectory _trajectory_type;
00021       _trajectory_type st_trajectory;
00022       _trajectory_type * trajectory;
00023       typedef moveit_msgs::RobotState _trajectory_start_type;
00024       _trajectory_start_type trajectory_start;
00025 
00026     DisplayTrajectory():
00027       model_id(""),
00028       trajectory_length(0), trajectory(NULL),
00029       trajectory_start()
00030     {
00031     }
00032 
00033     virtual int serialize(unsigned char *outbuffer) const
00034     {
00035       int offset = 0;
00036       uint32_t length_model_id = strlen(this->model_id);
00037       varToArr(outbuffer + offset, length_model_id);
00038       offset += 4;
00039       memcpy(outbuffer + offset, this->model_id, length_model_id);
00040       offset += length_model_id;
00041       *(outbuffer + offset + 0) = (this->trajectory_length >> (8 * 0)) & 0xFF;
00042       *(outbuffer + offset + 1) = (this->trajectory_length >> (8 * 1)) & 0xFF;
00043       *(outbuffer + offset + 2) = (this->trajectory_length >> (8 * 2)) & 0xFF;
00044       *(outbuffer + offset + 3) = (this->trajectory_length >> (8 * 3)) & 0xFF;
00045       offset += sizeof(this->trajectory_length);
00046       for( uint32_t i = 0; i < trajectory_length; i++){
00047       offset += this->trajectory[i].serialize(outbuffer + offset);
00048       }
00049       offset += this->trajectory_start.serialize(outbuffer + offset);
00050       return offset;
00051     }
00052 
00053     virtual int deserialize(unsigned char *inbuffer)
00054     {
00055       int offset = 0;
00056       uint32_t length_model_id;
00057       arrToVar(length_model_id, (inbuffer + offset));
00058       offset += 4;
00059       for(unsigned int k= offset; k< offset+length_model_id; ++k){
00060           inbuffer[k-1]=inbuffer[k];
00061       }
00062       inbuffer[offset+length_model_id-1]=0;
00063       this->model_id = (char *)(inbuffer + offset-1);
00064       offset += length_model_id;
00065       uint32_t trajectory_lengthT = ((uint32_t) (*(inbuffer + offset))); 
00066       trajectory_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
00067       trajectory_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
00068       trajectory_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
00069       offset += sizeof(this->trajectory_length);
00070       if(trajectory_lengthT > trajectory_length)
00071         this->trajectory = (moveit_msgs::RobotTrajectory*)realloc(this->trajectory, trajectory_lengthT * sizeof(moveit_msgs::RobotTrajectory));
00072       trajectory_length = trajectory_lengthT;
00073       for( uint32_t i = 0; i < trajectory_length; i++){
00074       offset += this->st_trajectory.deserialize(inbuffer + offset);
00075         memcpy( &(this->trajectory[i]), &(this->st_trajectory), sizeof(moveit_msgs::RobotTrajectory));
00076       }
00077       offset += this->trajectory_start.deserialize(inbuffer + offset);
00078      return offset;
00079     }
00080 
00081     virtual const char * getType(){ return "moveit_msgs/DisplayTrajectory"; };
00082     virtual const char * getMD5(){ return "c3c039261ab9e8a11457dac56b6316c8"; };
00083 
00084   };
00085 
00086 }
00087 #endif