41 #include <boost/shared_ptr.hpp> 44 #include <control_msgs/FollowJointTrajectoryAction.h> 45 #include <trajectory_msgs/JointTrajectoryPoint.h> 65 inline std::vector<unsigned int>
mapping(
const T& t1,
const T& t2)
67 typedef unsigned int SizeType;
70 if (t1.size() > t2.size()) {
return std::vector<SizeType>();}
72 std::vector<SizeType> mapping_vector(t1.size());
73 for (
typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
75 typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
76 if (t2.end() == t2_it) {
return std::vector<SizeType>();}
79 const SizeType t1_dist = std::distance(t1.begin(), t1_it);
80 const SizeType t2_dist = std::distance(t2.begin(), t2_it);
81 mapping_vector[t1_dist] = t2_dist;
84 return mapping_vector;
93 template <
class Trajectory>
99 typedef typename TrajectoryPerJoint::value_type
Segment;
103 : current_trajectory(nullptr),
104 joint_names(nullptr),
105 angle_wraparound(nullptr),
107 default_tolerances(nullptr),
108 other_time_base(nullptr),
109 allow_partial_joints_goal(false),
110 error_string(nullptr)
131 template <
class Trajectory>
132 bool isNotEmpty(
typename Trajectory::value_type trajPerJoint)
134 return !trajPerJoint.empty();
203 template <
class Trajectory>
209 typedef typename Trajectory::value_type TrajectoryPerJoint;
210 typedef typename TrajectoryPerJoint::value_type Segment;
211 typedef typename Segment::Scalar Scalar;
212 typedef typename TrajectoryPerJoint::const_iterator TrajIter;
214 const unsigned int n_joints = msg.joint_names.size();
219 << std::fixed << std::setprecision(3) << msg_start_time.
toSec());
221 std::string error_string;
224 if (msg.points.empty())
226 error_string =
"Trajectory message contains empty trajectory. Nothing to convert.";
228 options.setErrorString(error_string);
235 error_string =
"Trajectory message contains waypoints that are not strictly increasing in time.";
237 options.setErrorString(error_string);
242 const bool has_current_trajectory =
options.current_trajectory && !
options.current_trajectory->empty();
243 const bool has_joint_names =
options.joint_names && !
options.joint_names->empty();
244 const bool has_angle_wraparound =
options.angle_wraparound && !
options.angle_wraparound->empty();
245 const bool has_rt_goal_handle =
options.rt_goal_handle !=
nullptr;
246 const bool has_other_time_base =
options.other_time_base !=
nullptr;
247 const bool has_default_tolerances =
options.default_tolerances !=
nullptr;
249 if (!has_current_trajectory && has_angle_wraparound)
251 ROS_WARN(
"Vector specifying whether joints wrap around will not be used because no current trajectory was given.");
258 if (has_other_time_base)
261 o_time = *
options.other_time_base;
262 o_msg_start_time = o_time + msg_start_duration;
263 ROS_DEBUG_STREAM(
"Using alternate time base. In it, the new trajectory starts at time " 264 << std::fixed << std::setprecision(3) << o_msg_start_time.
toSec());
269 o_msg_start_time = msg_start_time;
272 const std::vector<std::string> joint_names = has_joint_names ? *(
options.joint_names) : msg.joint_names;
274 if (has_angle_wraparound)
277 const unsigned int n_angle_wraparound =
options.angle_wraparound->size();
278 if (n_angle_wraparound != joint_names.size())
280 error_string =
"Cannot create trajectory from message. " 281 "Vector specifying whether joints wrap around has an invalid size.";
283 options.setErrorString(error_string);
289 if (!
options.allow_partial_joints_goal)
291 if (msg.joint_names.size() != joint_names.size())
293 error_string =
"Cannot create trajectory from message. It does not contain the expected joints.";
295 options.setErrorString(error_string);
302 std::vector<unsigned int> mapping_vector =
internal::mapping(msg.joint_names,joint_names);
304 if (mapping_vector.empty())
306 error_string =
"Cannot create trajectory from message. It does not contain the expected joints.";
308 options.setErrorString(error_string);
316 if (has_rt_goal_handle &&
options.rt_goal_handle->gh_.getGoal())
318 updateSegmentTolerances<Scalar>(*(
options.rt_goal_handle->gh_.getGoal()), joint_names, tolerances);
324 std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator
326 if (msg_it == msg.points.end())
328 msg_it = msg.points.begin();
333 if (msg_it == msg.points.end())
335 ros::Duration last_point_dur = time - (msg_start_time + (--msg_it)->time_from_start);
336 error_string =
"Dropping all " + std::to_string(msg.points.size());
337 error_string +=
" trajectory point(s), as they occur before the current time.\n";
338 error_string +=
"Last point is " + std::to_string(last_point_dur.
toSec());
339 error_string +=
"s in the past.";
341 options.setErrorString(error_string);
345 msg.points.begin()->time_from_start.isZero() &&
346 msg.header.stamp.isZero() &&
347 std::distance(msg.points.begin(), msg_it) == 1
351 "First valid point will be reached at time_from_start " <<
352 std::fixed << std::setprecision(3) << msg_it->time_from_start.toSec() <<
"s.");
356 ros::Duration next_point_dur = msg_start_time + msg_it->time_from_start - time;
357 ROS_WARN_STREAM(
"Dropping first " << std::distance(msg.points.begin(), msg_it) <<
358 " trajectory point(s) out of " << msg.points.size() <<
359 ", as they occur before the current time.\n" <<
360 "First valid point will be reached in " << std::fixed << std::setprecision(3) <<
361 next_point_dur.
toSec() <<
"s.");
368 Trajectory result_traj;
371 if (has_current_trajectory)
373 result_traj = *(
options.current_trajectory);
376 for (
unsigned int joint_id=0; joint_id < joint_names.size();joint_id++)
378 const TrajectoryPerJoint& curr_joint_traj = result_traj[joint_id];
381 while (std::distance(active_seg, curr_joint_traj.end())>=1)
383 (result_traj[joint_id])[std::distance(curr_joint_traj.begin(),active_seg)].setGoalHandle(
options.rt_goal_handle);
389 result_traj.resize(joint_names.size());
393 for (
unsigned int msg_joint_it=0; msg_joint_it < mapping_vector.size();msg_joint_it++)
395 std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator it = msg_it;
396 if (!
isValid(*it, it->positions.size()))
397 throw(std::invalid_argument(
"Size mismatch in trajectory point position, velocity or acceleration data."));
399 TrajectoryPerJoint result_traj_per_joint;
400 unsigned int joint_id = mapping_vector[msg_joint_it];
403 std::vector<Scalar> position_offset(1, 0.0);
412 if (has_current_trajectory)
414 const TrajectoryPerJoint& curr_joint_traj = (*
options.current_trajectory)[joint_id];
417 const typename Segment::Time last_curr_time = std::max(o_msg_start_time.
toSec(), o_time.
toSec());
418 typename Segment::State last_curr_state;
419 sample(curr_joint_traj, last_curr_time, last_curr_state);
422 trajectory_msgs::JointTrajectoryPoint point_per_joint;
423 if (!it->positions.empty()) {point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
424 if (!it->velocities.empty()) {point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
425 if (!it->accelerations.empty()) {point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
426 point_per_joint.time_from_start = it->time_from_start;
428 const typename Segment::Time first_new_time = o_msg_start_time.
toSec() + (it->time_from_start).toSec();
429 typename Segment::State first_new_state(point_per_joint);
432 if (has_angle_wraparound)
435 first_new_state.position[0],
436 (*
options.angle_wraparound)[joint_id]);
440 first_new_state =
typename Segment::State(point_per_joint, position_offset);
445 TrajIter last =
findSegment(curr_joint_traj, last_curr_time);
446 if (first == curr_joint_traj.end() || last == curr_joint_traj.end())
448 error_string =
"Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.";
450 options.setErrorString(error_string);
453 result_traj_per_joint.insert(result_traj_per_joint.begin(), first, ++last);
457 Segment bridge_seg(last_curr_time, last_curr_state,
458 first_new_time, first_new_state);
459 bridge_seg.setGoalHandle(
options.rt_goal_handle);
460 if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances_per_joint);}
461 result_traj_per_joint.push_back(bridge_seg);
465 const unsigned int num_old_segments = result_traj_per_joint.size() -1;
466 const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1;
471 while (std::distance(it, msg.points.end()) >= 2)
473 std::vector<trajectory_msgs::JointTrajectoryPoint>::const_iterator next_it = it; ++next_it;
475 trajectory_msgs::JointTrajectoryPoint it_point_per_joint, next_it_point_per_joint;
477 if (!
isValid(*it, it->positions.size()))
478 throw(std::invalid_argument(
"Size mismatch in trajectory point position, velocity or acceleration data."));
479 if (!it->positions.empty()) {it_point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
480 if (!it->velocities.empty()) {it_point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
481 if (!it->accelerations.empty()) {it_point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
482 it_point_per_joint.time_from_start = it->time_from_start;
484 if (!
isValid(*next_it, next_it->positions.size()))
485 throw(std::invalid_argument(
"Size mismatch in trajectory point position, velocity or acceleration data."));
486 if (!next_it->positions.empty()) {next_it_point_per_joint.positions.resize(1, next_it->positions[msg_joint_it]);}
487 if (!next_it->velocities.empty()) {next_it_point_per_joint.velocities.resize(1, next_it->velocities[msg_joint_it]);}
488 if (!next_it->accelerations.empty()) {next_it_point_per_joint.accelerations.resize(1, next_it->accelerations[msg_joint_it]);}
489 next_it_point_per_joint.time_from_start = next_it->time_from_start;
491 Segment segment(o_msg_start_time, it_point_per_joint, next_it_point_per_joint, position_offset);
492 segment.setGoalHandle(
options.rt_goal_handle);
493 if (has_rt_goal_handle) {segment.setTolerances(tolerances_per_joint);}
494 result_traj_per_joint.push_back(segment);
499 std::stringstream log_str;
500 log_str <<
"Trajectory of joint " << joint_names[joint_id] <<
"has " << result_traj_per_joint.size() <<
" segments";
501 if (has_current_trajectory)
504 log_str <<
"\n- " << num_old_segments <<
" segment(s) will still be executed from previous trajectory.";
505 log_str <<
"\n- 1 segment added for transitioning between the current trajectory and first point of the input message.";
506 if (num_new_segments > 0) {log_str <<
"\n- " << num_new_segments <<
" new segments (" << (num_new_segments + 1) <<
507 " points) taken from the input trajectory.";}
509 else {log_str <<
".";}
512 if (result_traj_per_joint.size() > 0)
513 result_traj[joint_id] = result_traj_per_joint;
517 typename Trajectory::const_iterator trajIter = std::find_if (result_traj.begin(), result_traj.end(), isNotEmpty<Trajectory>);
518 if (trajIter == result_traj.end())
std::vector< StateTolerances< Scalar > > state_tolerance
Scalar goal_time_tolerance
StateTolerances< Scalar > state_tolerance
Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory &msg, const ros::Time &time, const InitJointTrajectoryOptions< Trajectory > &options=InitJointTrajectoryOptions< Trajectory >())
Initialize a joint trajectory from ROS message data.
RealtimeGoalHandlePtr rt_goal_handle
InitJointTrajectoryOptions()
boost::shared_ptr< RealtimeGoalHandle > RealtimeGoalHandlePtr
bool allow_partial_joints_goal
Trajectory::const_iterator sample(const Trajectory &trajectory, const typename Trajectory::value_type::Time &time, typename Trajectory::value_type::State &state)
Sample a trajectory at a specified time.
ros::Time * other_time_base
Trajectory * current_trajectory
TrajectoryIterator findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time &time)
Find an iterator to the segment containing a specified time.
std::vector< bool > * angle_wraparound
Trajectory::value_type TrajectoryPerJoint
bool isTimeStrictlyIncreasing(const trajectory_msgs::JointTrajectory &msg)
SegmentTolerances< Scalar > * default_tolerances
Scalar goal_time_tolerance
StateTolerances< Scalar > goal_state_tolerance
Scalar wraparoundJointOffset(const Scalar &prev_position, const Scalar &next_position, const bool &angle_wraparound)
TrajectoryPerJoint::value_type Segment
realtime_tools::RealtimeServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RealtimeGoalHandle
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
std::string * error_string
std::vector< unsigned int > mapping(const T &t1, const T &t2)
bool isNotEmpty(typename Trajectory::value_type trajPerJoint)
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.
Options used when initializing a joint trajectory from ROS message data.
Trajectory segment tolerances per Joint.
#define ROS_ERROR_STREAM(args)
std::vector< StateTolerances< Scalar > > goal_state_tolerance
ros::Time startTime(const trajectory_msgs::JointTrajectory &msg, const ros::Time &time)
bool isValid(const trajectory_msgs::JointTrajectoryPoint &point, const unsigned int joint_dim)
void setErrorString(const std::string &msg) const
Trajectory segment tolerances.
std::vector< std::string > * joint_names