This is a fork from the original, including a small change in the buffer size of the hardware interface (increased to 2048) and decreasing the number of publishers and subscribers to 5. Besides, the library about the message Adc.h was modified so as to increase the number of available Adc channels to be read ( from 6 to 7 ) For this modification, a change in checksum was required

Dependencies:   BufferedSerial

Fork of ros_lib_kinetic by Gary Servin

Committer:
jacobepfl1692
Date:
Tue Oct 17 18:49:03 2017 +0000
Revision:
2:9114cc24ddcf
Parent:
0:9e9b7db60fd5
I increased the channels of the ADC to 6 (hence change in checksum) because my application needed it (STM32f407V6)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garyservin 0:9e9b7db60fd5 1 #ifndef _ROS_control_msgs_FollowJointTrajectoryGoal_h
garyservin 0:9e9b7db60fd5 2 #define _ROS_control_msgs_FollowJointTrajectoryGoal_h
garyservin 0:9e9b7db60fd5 3
garyservin 0:9e9b7db60fd5 4 #include <stdint.h>
garyservin 0:9e9b7db60fd5 5 #include <string.h>
garyservin 0:9e9b7db60fd5 6 #include <stdlib.h>
garyservin 0:9e9b7db60fd5 7 #include "ros/msg.h"
garyservin 0:9e9b7db60fd5 8 #include "trajectory_msgs/JointTrajectory.h"
garyservin 0:9e9b7db60fd5 9 #include "control_msgs/JointTolerance.h"
garyservin 0:9e9b7db60fd5 10 #include "ros/duration.h"
garyservin 0:9e9b7db60fd5 11
garyservin 0:9e9b7db60fd5 12 namespace control_msgs
garyservin 0:9e9b7db60fd5 13 {
garyservin 0:9e9b7db60fd5 14
garyservin 0:9e9b7db60fd5 15 class FollowJointTrajectoryGoal : public ros::Msg
garyservin 0:9e9b7db60fd5 16 {
garyservin 0:9e9b7db60fd5 17 public:
garyservin 0:9e9b7db60fd5 18 typedef trajectory_msgs::JointTrajectory _trajectory_type;
garyservin 0:9e9b7db60fd5 19 _trajectory_type trajectory;
garyservin 0:9e9b7db60fd5 20 uint32_t path_tolerance_length;
garyservin 0:9e9b7db60fd5 21 typedef control_msgs::JointTolerance _path_tolerance_type;
garyservin 0:9e9b7db60fd5 22 _path_tolerance_type st_path_tolerance;
garyservin 0:9e9b7db60fd5 23 _path_tolerance_type * path_tolerance;
garyservin 0:9e9b7db60fd5 24 uint32_t goal_tolerance_length;
garyservin 0:9e9b7db60fd5 25 typedef control_msgs::JointTolerance _goal_tolerance_type;
garyservin 0:9e9b7db60fd5 26 _goal_tolerance_type st_goal_tolerance;
garyservin 0:9e9b7db60fd5 27 _goal_tolerance_type * goal_tolerance;
garyservin 0:9e9b7db60fd5 28 typedef ros::Duration _goal_time_tolerance_type;
garyservin 0:9e9b7db60fd5 29 _goal_time_tolerance_type goal_time_tolerance;
garyservin 0:9e9b7db60fd5 30
garyservin 0:9e9b7db60fd5 31 FollowJointTrajectoryGoal():
garyservin 0:9e9b7db60fd5 32 trajectory(),
garyservin 0:9e9b7db60fd5 33 path_tolerance_length(0), path_tolerance(NULL),
garyservin 0:9e9b7db60fd5 34 goal_tolerance_length(0), goal_tolerance(NULL),
garyservin 0:9e9b7db60fd5 35 goal_time_tolerance()
garyservin 0:9e9b7db60fd5 36 {
garyservin 0:9e9b7db60fd5 37 }
garyservin 0:9e9b7db60fd5 38
garyservin 0:9e9b7db60fd5 39 virtual int serialize(unsigned char *outbuffer) const
garyservin 0:9e9b7db60fd5 40 {
garyservin 0:9e9b7db60fd5 41 int offset = 0;
garyservin 0:9e9b7db60fd5 42 offset += this->trajectory.serialize(outbuffer + offset);
garyservin 0:9e9b7db60fd5 43 *(outbuffer + offset + 0) = (this->path_tolerance_length >> (8 * 0)) & 0xFF;
garyservin 0:9e9b7db60fd5 44 *(outbuffer + offset + 1) = (this->path_tolerance_length >> (8 * 1)) & 0xFF;
garyservin 0:9e9b7db60fd5 45 *(outbuffer + offset + 2) = (this->path_tolerance_length >> (8 * 2)) & 0xFF;
garyservin 0:9e9b7db60fd5 46 *(outbuffer + offset + 3) = (this->path_tolerance_length >> (8 * 3)) & 0xFF;
garyservin 0:9e9b7db60fd5 47 offset += sizeof(this->path_tolerance_length);
garyservin 0:9e9b7db60fd5 48 for( uint32_t i = 0; i < path_tolerance_length; i++){
garyservin 0:9e9b7db60fd5 49 offset += this->path_tolerance[i].serialize(outbuffer + offset);
garyservin 0:9e9b7db60fd5 50 }
garyservin 0:9e9b7db60fd5 51 *(outbuffer + offset + 0) = (this->goal_tolerance_length >> (8 * 0)) & 0xFF;
garyservin 0:9e9b7db60fd5 52 *(outbuffer + offset + 1) = (this->goal_tolerance_length >> (8 * 1)) & 0xFF;
garyservin 0:9e9b7db60fd5 53 *(outbuffer + offset + 2) = (this->goal_tolerance_length >> (8 * 2)) & 0xFF;
garyservin 0:9e9b7db60fd5 54 *(outbuffer + offset + 3) = (this->goal_tolerance_length >> (8 * 3)) & 0xFF;
garyservin 0:9e9b7db60fd5 55 offset += sizeof(this->goal_tolerance_length);
garyservin 0:9e9b7db60fd5 56 for( uint32_t i = 0; i < goal_tolerance_length; i++){
garyservin 0:9e9b7db60fd5 57 offset += this->goal_tolerance[i].serialize(outbuffer + offset);
garyservin 0:9e9b7db60fd5 58 }
garyservin 0:9e9b7db60fd5 59 *(outbuffer + offset + 0) = (this->goal_time_tolerance.sec >> (8 * 0)) & 0xFF;
garyservin 0:9e9b7db60fd5 60 *(outbuffer + offset + 1) = (this->goal_time_tolerance.sec >> (8 * 1)) & 0xFF;
garyservin 0:9e9b7db60fd5 61 *(outbuffer + offset + 2) = (this->goal_time_tolerance.sec >> (8 * 2)) & 0xFF;
garyservin 0:9e9b7db60fd5 62 *(outbuffer + offset + 3) = (this->goal_time_tolerance.sec >> (8 * 3)) & 0xFF;
garyservin 0:9e9b7db60fd5 63 offset += sizeof(this->goal_time_tolerance.sec);
garyservin 0:9e9b7db60fd5 64 *(outbuffer + offset + 0) = (this->goal_time_tolerance.nsec >> (8 * 0)) & 0xFF;
garyservin 0:9e9b7db60fd5 65 *(outbuffer + offset + 1) = (this->goal_time_tolerance.nsec >> (8 * 1)) & 0xFF;
garyservin 0:9e9b7db60fd5 66 *(outbuffer + offset + 2) = (this->goal_time_tolerance.nsec >> (8 * 2)) & 0xFF;
garyservin 0:9e9b7db60fd5 67 *(outbuffer + offset + 3) = (this->goal_time_tolerance.nsec >> (8 * 3)) & 0xFF;
garyservin 0:9e9b7db60fd5 68 offset += sizeof(this->goal_time_tolerance.nsec);
garyservin 0:9e9b7db60fd5 69 return offset;
garyservin 0:9e9b7db60fd5 70 }
garyservin 0:9e9b7db60fd5 71
garyservin 0:9e9b7db60fd5 72 virtual int deserialize(unsigned char *inbuffer)
garyservin 0:9e9b7db60fd5 73 {
garyservin 0:9e9b7db60fd5 74 int offset = 0;
garyservin 0:9e9b7db60fd5 75 offset += this->trajectory.deserialize(inbuffer + offset);
garyservin 0:9e9b7db60fd5 76 uint32_t path_tolerance_lengthT = ((uint32_t) (*(inbuffer + offset)));
garyservin 0:9e9b7db60fd5 77 path_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
garyservin 0:9e9b7db60fd5 78 path_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
garyservin 0:9e9b7db60fd5 79 path_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
garyservin 0:9e9b7db60fd5 80 offset += sizeof(this->path_tolerance_length);
garyservin 0:9e9b7db60fd5 81 if(path_tolerance_lengthT > path_tolerance_length)
garyservin 0:9e9b7db60fd5 82 this->path_tolerance = (control_msgs::JointTolerance*)realloc(this->path_tolerance, path_tolerance_lengthT * sizeof(control_msgs::JointTolerance));
garyservin 0:9e9b7db60fd5 83 path_tolerance_length = path_tolerance_lengthT;
garyservin 0:9e9b7db60fd5 84 for( uint32_t i = 0; i < path_tolerance_length; i++){
garyservin 0:9e9b7db60fd5 85 offset += this->st_path_tolerance.deserialize(inbuffer + offset);
garyservin 0:9e9b7db60fd5 86 memcpy( &(this->path_tolerance[i]), &(this->st_path_tolerance), sizeof(control_msgs::JointTolerance));
garyservin 0:9e9b7db60fd5 87 }
garyservin 0:9e9b7db60fd5 88 uint32_t goal_tolerance_lengthT = ((uint32_t) (*(inbuffer + offset)));
garyservin 0:9e9b7db60fd5 89 goal_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
garyservin 0:9e9b7db60fd5 90 goal_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
garyservin 0:9e9b7db60fd5 91 goal_tolerance_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
garyservin 0:9e9b7db60fd5 92 offset += sizeof(this->goal_tolerance_length);
garyservin 0:9e9b7db60fd5 93 if(goal_tolerance_lengthT > goal_tolerance_length)
garyservin 0:9e9b7db60fd5 94 this->goal_tolerance = (control_msgs::JointTolerance*)realloc(this->goal_tolerance, goal_tolerance_lengthT * sizeof(control_msgs::JointTolerance));
garyservin 0:9e9b7db60fd5 95 goal_tolerance_length = goal_tolerance_lengthT;
garyservin 0:9e9b7db60fd5 96 for( uint32_t i = 0; i < goal_tolerance_length; i++){
garyservin 0:9e9b7db60fd5 97 offset += this->st_goal_tolerance.deserialize(inbuffer + offset);
garyservin 0:9e9b7db60fd5 98 memcpy( &(this->goal_tolerance[i]), &(this->st_goal_tolerance), sizeof(control_msgs::JointTolerance));
garyservin 0:9e9b7db60fd5 99 }
garyservin 0:9e9b7db60fd5 100 this->goal_time_tolerance.sec = ((uint32_t) (*(inbuffer + offset)));
garyservin 0:9e9b7db60fd5 101 this->goal_time_tolerance.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
garyservin 0:9e9b7db60fd5 102 this->goal_time_tolerance.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
garyservin 0:9e9b7db60fd5 103 this->goal_time_tolerance.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
garyservin 0:9e9b7db60fd5 104 offset += sizeof(this->goal_time_tolerance.sec);
garyservin 0:9e9b7db60fd5 105 this->goal_time_tolerance.nsec = ((uint32_t) (*(inbuffer + offset)));
garyservin 0:9e9b7db60fd5 106 this->goal_time_tolerance.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
garyservin 0:9e9b7db60fd5 107 this->goal_time_tolerance.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
garyservin 0:9e9b7db60fd5 108 this->goal_time_tolerance.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
garyservin 0:9e9b7db60fd5 109 offset += sizeof(this->goal_time_tolerance.nsec);
garyservin 0:9e9b7db60fd5 110 return offset;
garyservin 0:9e9b7db60fd5 111 }
garyservin 0:9e9b7db60fd5 112
garyservin 0:9e9b7db60fd5 113 const char * getType(){ return "control_msgs/FollowJointTrajectoryGoal"; };
garyservin 0:9e9b7db60fd5 114 const char * getMD5(){ return "69636787b6ecbde4d61d711979bc7ecb"; };
garyservin 0:9e9b7db60fd5 115
garyservin 0:9e9b7db60fd5 116 };
garyservin 0:9e9b7db60fd5 117
garyservin 0:9e9b7db60fd5 118 }
garyservin 0:9e9b7db60fd5 119 #endif