Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00029
00030 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_MSG_UTILS_H
00031 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_MSG_UTILS_H
00032
00033 #include <algorithm>
00034 #include <iterator>
00035 #include <string>
00036 #include <vector>
00037
00038 #include <ros/console.h>
00039 #include <ros/time.h>
00040 #include <trajectory_msgs/JointTrajectory.h>
00041
00042 #include <trajectory_interface/trajectory_interface.h>
00043
00044 namespace joint_trajectory_controller
00045 {
00046 namespace internal
00047 {
00048
00049 class IsBeforePoint
00050 {
00051 public:
00052 IsBeforePoint(const ros::Time& msg_start_time) : msg_start_time_(msg_start_time) {}
00053
00054 bool operator()(const ros::Time& time, const trajectory_msgs::JointTrajectoryPoint& point)
00055 {
00056 const ros::Time point_start_time = msg_start_time_ + point.time_from_start;
00057 return time < point_start_time;
00058 }
00059
00060 private:
00061 ros::Time msg_start_time_;
00062 };
00063
00069 inline ros::Time startTime(const trajectory_msgs::JointTrajectory& msg,
00070 const ros::Time& time)
00071 {
00072 return msg.header.stamp.isZero() ? time : msg.header.stamp;
00073 }
00074
00075 }
00076
00083 inline bool isValid(const trajectory_msgs::JointTrajectoryPoint& point, const unsigned int joint_dim)
00084 {
00085 if (!point.positions.empty() && point.positions.size() != joint_dim) {return false;}
00086 if (!point.velocities.empty() && point.velocities.size() != joint_dim) {return false;}
00087 if (!point.accelerations.empty() && point.accelerations.size() != joint_dim) {return false;}
00088 return true;
00089 }
00090
00095 inline bool isValid(const trajectory_msgs::JointTrajectory& msg)
00096 {
00097 const std::vector<std::string>::size_type joint_dim = msg.joint_names.size();
00098
00099 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
00100
00101 for (PointConstIterator it = msg.points.begin(); it != msg.points.end(); ++it)
00102 {
00103 const std::iterator_traits<PointConstIterator>::difference_type index = std::distance(msg.points.begin(), it);
00104
00105 if(!isValid(*it, joint_dim))
00106 {
00107 ROS_ERROR_STREAM("Invalid trajectory point at index: " << index <<
00108 ". Size mismatch in joint names, position, velocity or acceleration data.");
00109 return false;
00110 }
00111 }
00112 return true;
00113 }
00114
00119 inline bool isTimeStrictlyIncreasing(const trajectory_msgs::JointTrajectory& msg)
00120 {
00121 if (msg.points.size() < 2) {return true;}
00122
00123 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
00124
00125 PointConstIterator it = msg.points.begin();
00126 PointConstIterator end_it = --msg.points.end();
00127 while (it != end_it)
00128 {
00129 const ros::Duration& t1 = it->time_from_start;
00130 const ros::Duration& t2 = (++it)->time_from_start;
00131 if (t1 >= t2) {return false;}
00132 }
00133 return true;
00134 }
00135
00150 inline std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator
00151 findPoint(const trajectory_msgs::JointTrajectory& msg,
00152 const ros::Time& time)
00153 {
00154
00155
00156 const ros::Time msg_start_time = internal::startTime(msg, time);
00157 internal::IsBeforePoint isBeforePoint(msg_start_time);
00158
00159 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator ConstIterator;
00160 const ConstIterator first = msg.points.begin();
00161 const ConstIterator last = msg.points.end();
00162
00163 return (first == last || isBeforePoint(time, *first))
00164 ? last
00165 : --std::upper_bound(first, last, time, isBeforePoint);
00166 }
00167
00168 }
00169
00170 #endif // header guard