Just changed OUTPUT_SIZE and INPUT_SIZE in ros/node_handle.h
Dependents: WRS2020_mecanum_node
JointTrajectory.h
00001 #ifndef _ROS_trajectory_msgs_JointTrajectory_h 00002 #define _ROS_trajectory_msgs_JointTrajectory_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "std_msgs/Header.h" 00009 #include "trajectory_msgs/JointTrajectoryPoint.h" 00010 00011 namespace trajectory_msgs 00012 { 00013 00014 class JointTrajectory : public ros::Msg 00015 { 00016 public: 00017 typedef std_msgs::Header _header_type; 00018 _header_type header; 00019 uint32_t joint_names_length; 00020 typedef char* _joint_names_type; 00021 _joint_names_type st_joint_names; 00022 _joint_names_type * joint_names; 00023 uint32_t points_length; 00024 typedef trajectory_msgs::JointTrajectoryPoint _points_type; 00025 _points_type st_points; 00026 _points_type * points; 00027 00028 JointTrajectory(): 00029 header(), 00030 joint_names_length(0), joint_names(NULL), 00031 points_length(0), points(NULL) 00032 { 00033 } 00034 00035 virtual int serialize(unsigned char *outbuffer) const 00036 { 00037 int offset = 0; 00038 offset += this->header.serialize(outbuffer + offset); 00039 *(outbuffer + offset + 0) = (this->joint_names_length >> (8 * 0)) & 0xFF; 00040 *(outbuffer + offset + 1) = (this->joint_names_length >> (8 * 1)) & 0xFF; 00041 *(outbuffer + offset + 2) = (this->joint_names_length >> (8 * 2)) & 0xFF; 00042 *(outbuffer + offset + 3) = (this->joint_names_length >> (8 * 3)) & 0xFF; 00043 offset += sizeof(this->joint_names_length); 00044 for( uint32_t i = 0; i < joint_names_length; i++){ 00045 uint32_t length_joint_namesi = strlen(this->joint_names[i]); 00046 varToArr(outbuffer + offset, length_joint_namesi); 00047 offset += 4; 00048 memcpy(outbuffer + offset, this->joint_names[i], length_joint_namesi); 00049 offset += length_joint_namesi; 00050 } 00051 *(outbuffer + offset + 0) = (this->points_length >> (8 * 0)) & 0xFF; 00052 *(outbuffer + offset + 1) = (this->points_length >> (8 * 1)) & 0xFF; 00053 *(outbuffer + offset + 2) = (this->points_length >> (8 * 2)) & 0xFF; 00054 *(outbuffer + offset + 3) = (this->points_length >> (8 * 3)) & 0xFF; 00055 offset += sizeof(this->points_length); 00056 for( uint32_t i = 0; i < points_length; i++){ 00057 offset += this->points[i].serialize(outbuffer + offset); 00058 } 00059 return offset; 00060 } 00061 00062 virtual int deserialize(unsigned char *inbuffer) 00063 { 00064 int offset = 0; 00065 offset += this->header.deserialize(inbuffer + offset); 00066 uint32_t joint_names_lengthT = ((uint32_t) (*(inbuffer + offset))); 00067 joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00068 joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00069 joint_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00070 offset += sizeof(this->joint_names_length); 00071 if(joint_names_lengthT > joint_names_length) 00072 this->joint_names = (char**)realloc(this->joint_names, joint_names_lengthT * sizeof(char*)); 00073 joint_names_length = joint_names_lengthT; 00074 for( uint32_t i = 0; i < joint_names_length; i++){ 00075 uint32_t length_st_joint_names; 00076 arrToVar(length_st_joint_names, (inbuffer + offset)); 00077 offset += 4; 00078 for(unsigned int k= offset; k< offset+length_st_joint_names; ++k){ 00079 inbuffer[k-1]=inbuffer[k]; 00080 } 00081 inbuffer[offset+length_st_joint_names-1]=0; 00082 this->st_joint_names = (char *)(inbuffer + offset-1); 00083 offset += length_st_joint_names; 00084 memcpy( &(this->joint_names[i]), &(this->st_joint_names), sizeof(char*)); 00085 } 00086 uint32_t points_lengthT = ((uint32_t) (*(inbuffer + offset))); 00087 points_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00088 points_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00089 points_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00090 offset += sizeof(this->points_length); 00091 if(points_lengthT > points_length) 00092 this->points = (trajectory_msgs::JointTrajectoryPoint*)realloc(this->points, points_lengthT * sizeof(trajectory_msgs::JointTrajectoryPoint)); 00093 points_length = points_lengthT; 00094 for( uint32_t i = 0; i < points_length; i++){ 00095 offset += this->st_points.deserialize(inbuffer + offset); 00096 memcpy( &(this->points[i]), &(this->st_points), sizeof(trajectory_msgs::JointTrajectoryPoint)); 00097 } 00098 return offset; 00099 } 00100 00101 const char * getType(){ return "trajectory_msgs/JointTrajectory"; }; 00102 const char * getMD5(){ return "65b4f94a94d1ed67169da35a02f33d3f"; }; 00103 00104 }; 00105 00106 } 00107 #endif
Generated on Tue Jul 12 2022 18:49:19 by 1.7.2