40 #include <trajectory_msgs/JointTrajectory.h> 57 return time < point_start_time;
72 return msg.header.stamp.
isZero() ? time : msg.header.stamp;
83 inline bool isValid(
const trajectory_msgs::JointTrajectoryPoint& point,
const unsigned int joint_dim)
85 if (!point.positions.empty() && point.positions.size() != joint_dim) {
return false;}
86 if (!point.velocities.empty() && point.velocities.size() != joint_dim) {
return false;}
87 if (!point.accelerations.empty() && point.accelerations.size() != joint_dim) {
return false;}
95 inline bool isValid(
const trajectory_msgs::JointTrajectory& msg)
97 const std::vector<std::string>::size_type joint_dim = msg.joint_names.size();
99 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
101 for (PointConstIterator it = msg.points.begin(); it != msg.points.end(); ++it)
103 const std::iterator_traits<PointConstIterator>::difference_type index = std::distance(msg.points.begin(), it);
108 ". Size mismatch in joint names, position, velocity or acceleration data.");
121 if (msg.points.size() < 2) {
return true;}
123 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator PointConstIterator;
125 PointConstIterator it = msg.points.begin();
126 PointConstIterator end_it = --msg.points.end();
131 if (t1 >= t2) {
return false;}
150 inline std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator
159 typedef std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator ConstIterator;
160 const ConstIterator first = msg.points.begin();
161 const ConstIterator last = msg.points.end();
163 return (first == last || isBeforePoint(time, *first))
165 : --std::upper_bound(first, last, time, isBeforePoint);
IsBeforePoint(const ros::Time &msg_start_time)
bool isTimeStrictlyIncreasing(const trajectory_msgs::JointTrajectory &msg)
bool operator()(const ros::Time &time, const trajectory_msgs::JointTrajectoryPoint &point)
std::vector< trajectory_msgs::JointTrajectoryPoint >::const_iterator findPoint(const trajectory_msgs::JointTrajectory &msg, const ros::Time &time)
Find an iterator to the trajectory point with the greatest start time < time.
#define ROS_ERROR_STREAM(args)
ros::Time msg_start_time_
ros::Time startTime(const trajectory_msgs::JointTrajectory &msg, const ros::Time &time)
bool isValid(const trajectory_msgs::JointTrajectoryPoint &point, const unsigned int joint_dim)