diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
index 75cf1889f..37269efb6 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
@@ -57,19 +57,19 @@ namespace joint_trajectory_controller
namespace internal
{
/**
- * \return The permutation vector between two containers.
- * If \p t1 is "{A, B, C, D}" and \p t2 is "{B, D, A, C}", the associated permutation vector is
- * "{2, 0, 3, 1}".
+ * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices.
+ * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is
+ * "{2, 1}".
*/
template
-inline std::vector permutation(const T& t1, const T& t2)
+inline std::vector mapping(const T& t1, const T& t2)
{
typedef unsigned int SizeType;
+
+ // t1 must be a subset of t2
+ if (t1.size() > t2.size()) {return std::vector();}
- // Arguments must have the same size
- if (t1.size() != t2.size()) {return std::vector();}
-
- std::vector permutation_vector(t1.size()); // Return value
+ std::vector mapping_vector(t1.size()); // Return value
for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
{
typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
@@ -78,10 +78,10 @@ inline std::vector permutation(const T& t1, const T& t2)
{
const SizeType t1_dist = std::distance(t1.begin(), t1_it);
const SizeType t2_dist = std::distance(t2.begin(), t2_it);
- permutation_vector[t1_dist] = t2_dist;
+ mapping_vector[t1_dist] = t2_dist;
}
}
- return permutation_vector;
+ return mapping_vector;
}
} // namespace
@@ -95,7 +95,9 @@ struct InitJointTrajectoryOptions
{
typedef realtime_tools::RealtimeServerGoalHandle RealtimeGoalHandle;
typedef boost::shared_ptr RealtimeGoalHandlePtr;
- typedef typename Trajectory::value_type::Scalar Scalar;
+ typedef typename Trajectory::value_type TrajectoryPerJoint;
+ typedef typename TrajectoryPerJoint::value_type Segment;
+ typedef typename Segment::Scalar Scalar;
InitJointTrajectoryOptions()
: current_trajectory(0),
@@ -103,7 +105,8 @@ struct InitJointTrajectoryOptions
angle_wraparound(0),
rt_goal_handle(),
default_tolerances(0),
- other_time_base(0)
+ other_time_base(0),
+ allow_partial_joints_goal(false)
{}
Trajectory* current_trajectory;
@@ -112,6 +115,13 @@ struct InitJointTrajectoryOptions
RealtimeGoalHandlePtr rt_goal_handle;
SegmentTolerances* default_tolerances;
ros::Time* other_time_base;
+ bool allow_partial_joints_goal;
+};
+
+template
+bool isNotEmpty(typename Trajectory::value_type trajPerJoint)
+{
+ return !trajPerJoint.empty();
};
/**
@@ -164,7 +174,6 @@ struct InitJointTrajectoryOptions
* Segment(const ros::Time& traj_start_time,
* const trajectory_msgs::JointTrajectoryPoint& start_point,
* const trajectory_msgs::JointTrajectoryPoint& end_point,
- * const std::vector& permutation,
* const std::vector& position_offset)
* \endcode
* The following function must also be defined to properly handle continuous joints:
@@ -184,8 +193,10 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
const InitJointTrajectoryOptions& options =
InitJointTrajectoryOptions())
{
- typedef typename Trajectory::value_type Segment;
+ typedef typename Trajectory::value_type TrajectoryPerJoint;
+ typedef typename TrajectoryPerJoint::value_type Segment;
typedef typename Segment::Scalar Scalar;
+ typedef typename TrajectoryPerJoint::const_iterator TrajIter;
const unsigned int n_joints = msg.joint_names.size();
@@ -239,13 +250,35 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
o_msg_start_time = msg_start_time;
}
- // Permutation vector mapping the expected joint order to the message joint order
- // If unspecified, a trivial map (no permutation) is computed
const std::vector joint_names = has_joint_names ? *(options.joint_names) : msg.joint_names;
- std::vector permutation_vector = internal::permutation(joint_names, msg.joint_names);
+ if (has_angle_wraparound)
+ {
+ // Preconditions
+ const unsigned int n_angle_wraparound = options.angle_wraparound->size();
+ if (n_angle_wraparound != joint_names.size())
+ {
+ ROS_ERROR("Cannot create trajectory from message. "
+ "Vector specifying whether joints wrap around has an invalid size.");
+ return Trajectory();
+ }
+ }
+
+ // If partial joints goals are not allowed, goal should specify all controller joints
+ if (!options.allow_partial_joints_goal)
+ {
+ if (msg.joint_names.size() != joint_names.size())
+ {
+ ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints.");
+ return Trajectory();
+ }
+ }
+
+ // Mapping vector contains the map between the message joint order and the expected joint order
+ // If unspecified, a trivial map is computed
+ std::vector mapping_vector = internal::mapping(msg.joint_names,joint_names);
- if (permutation_vector.empty())
+ if (mapping_vector.empty())
{
ROS_ERROR("Cannot create trajectory from message. It does not contain the expected joints.");
return Trajectory();
@@ -264,17 +297,17 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
// This point is used later on in this function, but is computed here, in advance because if the trajectory message
// contains a trajectory in the past, we can quickly return without spending additional computational resources
std::vector::const_iterator
- it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time)
- if (it == msg.points.end())
+ msg_it = findPoint(msg, time); // Points to last point occurring before current time (NOTE: Using time, not o_time)
+ if (msg_it == msg.points.end())
{
- it = msg.points.begin(); // Entire trajectory is after current time
+ msg_it = msg.points.begin(); // Entire trajectory is after current time
}
else
{
- ++it; // Points to first point after current time OR sequence end
- if (it == msg.points.end())
+ ++msg_it; // Points to first point after current time OR sequence end
+ if (msg_it == msg.points.end())
{
- ros::Duration last_point_dur = time - (msg_start_time + (--it)->time_from_start);
+ ros::Duration last_point_dur = time - (msg_start_time + (--msg_it)->time_from_start);
ROS_WARN_STREAM("Dropping all " << msg.points.size() <<
" trajectory point(s), as they occur before the current time.\n" <<
"Last point is " << std::fixed << std::setprecision(3) << last_point_dur.toSec() <<
@@ -283,8 +316,8 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
}
else
{
- ros::Duration next_point_dur = msg_start_time + it->time_from_start - time;
- ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), it) <<
+ ros::Duration next_point_dur = msg_start_time + msg_it->time_from_start - time;
+ ROS_WARN_STREAM("Dropping first " << std::distance(msg.points.begin(), msg_it) <<
" trajectory point(s) out of " << msg.points.size() <<
", as they occur before the current time.\n" <<
"First valid point will be reached in " << std::fixed << std::setprecision(3) <<
@@ -297,91 +330,156 @@ Trajectory initJointTrajectory(const trajectory_msgs::JointTrajectory& msg
// - Useful segments of new trajectory (contained in ROS message)
Trajectory result_traj; // Currently empty
- // Initialize offsets due to wrapping joints to zero
- std::vector position_offset(n_joints, 0.0);
-
- // Bridge current trajectory to new one
+ // Set active goal to segments after current time
if (has_current_trajectory)
{
- const Trajectory& curr_traj = *(options.current_trajectory);
-
- // Get the last time and state that will be executed from the current trajectory
- const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important!
- typename Segment::State last_curr_state;
- sample(curr_traj, last_curr_time, last_curr_state);
-
- // Get the first time and state that will be executed from the new trajectory
- const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec();
- typename Segment::State first_new_state(*it, permutation_vector); // Here offsets are not yet applied
+ result_traj = *(options.current_trajectory);
- // Compute offsets due to wrapping joints
- if (has_angle_wraparound)
+ //Iterate to all segments after current time to set the new goal handler
+ for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++)
{
- position_offset = wraparoundOffset(last_curr_state.position,
- first_new_state.position,
- *(options.angle_wraparound));
- if (position_offset.empty())
+ const TrajectoryPerJoint& curr_joint_traj = result_traj[joint_id];
+ TrajIter active_seg = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment
+
+ while (std::distance(active_seg, curr_joint_traj.end())>=1)
{
- ROS_ERROR("Cannot create trajectory from message. "
- "Vector specifying whether joints wrap around has an invalid size.");
- return Trajectory();
+ (result_traj[joint_id])[std::distance(curr_joint_traj.begin(),active_seg)].setGoalHandle(options.rt_goal_handle);
+ ++active_seg;
}
}
+ }
+ else
+ result_traj.resize(joint_names.size());
+
+ //Iterate through the joints that are in the message, in the order of the mapping vector
+ //for (unsigned int joint_id=0; joint_id < joint_names.size();joint_id++)
+ for (unsigned int msg_joint_it=0; msg_joint_it < mapping_vector.size();msg_joint_it++)
+ {
+ std::vector::const_iterator it = msg_it;
+ if (!isValid(*it, it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
- // Apply offset to first state that will be executed from the new trajectory
- first_new_state = typename Segment::State(*it, permutation_vector, position_offset); // Now offsets are applied
+ TrajectoryPerJoint result_traj_per_joint; // Currently empty
+ unsigned int joint_id = mapping_vector[msg_joint_it];
- // Add useful segments of current trajectory to result
+ // Initialize offsets due to wrapping joints to zero
+ std::vector position_offset(1, 0.0);
+
+ //Initialize segment tolerance per joint
+ SegmentTolerancesPerJoint tolerances_per_joint;
+ tolerances_per_joint.state_tolerance = tolerances.state_tolerance[joint_id];
+ tolerances_per_joint.goal_state_tolerance = tolerances.goal_state_tolerance[joint_id];
+ tolerances_per_joint.goal_time_tolerance = tolerances.goal_time_tolerance;
+
+ // Bridge current trajectory to new one
+ if (has_current_trajectory)
{
- typedef typename Trajectory::const_iterator TrajIter;
- TrajIter first = findSegment(curr_traj, o_time.toSec()); // Currently active segment
- TrajIter last = findSegment(curr_traj, last_curr_time); // Segment active when new trajectory starts
- if (first == curr_traj.end() || last == curr_traj.end())
+ const TrajectoryPerJoint& curr_joint_traj = (*options.current_trajectory)[joint_id];
+
+ // Get the last time and state that will be executed from the current trajectory
+ const typename Segment::Time last_curr_time = std::max(o_msg_start_time.toSec(), o_time.toSec()); // Important!
+ typename Segment::State last_curr_state;
+ sample(curr_joint_traj, last_curr_time, last_curr_state);
+
+ // Get the first time and state that will be executed from the new trajectory
+ trajectory_msgs::JointTrajectoryPoint point_per_joint;
+ if (!it->positions.empty()) {point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
+ if (!it->velocities.empty()) {point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
+ if (!it->accelerations.empty()) {point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
+ point_per_joint.time_from_start = it->time_from_start;
+
+ const typename Segment::Time first_new_time = o_msg_start_time.toSec() + (it->time_from_start).toSec();
+ typename Segment::State first_new_state(point_per_joint); // Here offsets are not yet applied
+
+ // Compute offsets due to wrapping joints
+ if (has_angle_wraparound)
+ {
+ position_offset[0] = wraparoundJointOffset(last_curr_state.position[0],
+ first_new_state.position[0],
+ (*options.angle_wraparound)[joint_id]);
+ }
+
+ // Apply offset to first state that will be executed from the new trajectory
+ first_new_state = typename Segment::State(point_per_joint, position_offset); // Now offsets are applied
+
+ // Add useful segments of current trajectory to result
{
- ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.");
- return Trajectory();
+ TrajIter first = findSegment(curr_joint_traj, o_time.toSec()); // Currently active segment
+ TrajIter last = findSegment(curr_joint_traj, last_curr_time); // Segment active when new trajectory starts
+ if (first == curr_joint_traj.end() || last == curr_joint_traj.end())
+ {
+ ROS_ERROR("Unexpected error: Could not find segments in current trajectory. Please contact the package maintainer.");
+ return Trajectory();
+ }
+ result_traj_per_joint.insert(result_traj_per_joint.begin(), first, ++last); // Range [first,last) will still be executed
}
- result_traj.insert(result_traj.begin(), first, ++last); // Range [first,last) will still be executed
+
+ // Add segment bridging current and new trajectories to result
+ Segment bridge_seg(last_curr_time, last_curr_state,
+ first_new_time, first_new_state);
+ bridge_seg.setGoalHandle(options.rt_goal_handle);
+ if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances_per_joint);}
+ result_traj_per_joint.push_back(bridge_seg);
}
- // Add segment bridging current and new trajectories to result
- Segment bridge_seg(last_curr_time, last_curr_state,
- first_new_time, first_new_state);
- bridge_seg.setGoalHandle(options.rt_goal_handle);
- if (has_rt_goal_handle) {bridge_seg.setTolerances(tolerances);}
- result_traj.push_back(bridge_seg);
- }
+ // Constants used in log statement at the end
+ const unsigned int num_old_segments = result_traj_per_joint.size() -1;
+ const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1;
- // Constants used in log statement at the end
- const unsigned int num_old_segments = result_traj.size() -1;
- const unsigned int num_new_segments = std::distance(it, msg.points.end()) -1;
+ // Add useful segments of new trajectory to result
+ // - Construct all trajectory segments occurring after current time
+ // - As long as there remain two trajectory points we can construct the next trajectory segment
+ while (std::distance(it, msg.points.end()) >= 2)
+ {
+ std::vector::const_iterator next_it = it; ++next_it;
+
+ trajectory_msgs::JointTrajectoryPoint it_point_per_joint, next_it_point_per_joint;
+
+ if (!isValid(*it, it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
+ if (!it->positions.empty()) {it_point_per_joint.positions.resize(1, it->positions[msg_joint_it]);}
+ if (!it->velocities.empty()) {it_point_per_joint.velocities.resize(1, it->velocities[msg_joint_it]);}
+ if (!it->accelerations.empty()) {it_point_per_joint.accelerations.resize(1, it->accelerations[msg_joint_it]);}
+ it_point_per_joint.time_from_start = it->time_from_start;
+
+ if (!isValid(*next_it, next_it->positions.size()))
+ throw(std::invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
+ if (!next_it->positions.empty()) {next_it_point_per_joint.positions.resize(1, next_it->positions[msg_joint_it]);}
+ if (!next_it->velocities.empty()) {next_it_point_per_joint.velocities.resize(1, next_it->velocities[msg_joint_it]);}
+ if (!next_it->accelerations.empty()) {next_it_point_per_joint.accelerations.resize(1, next_it->accelerations[msg_joint_it]);}
+ next_it_point_per_joint.time_from_start = next_it->time_from_start;
+
+ Segment segment(o_msg_start_time, it_point_per_joint, next_it_point_per_joint, position_offset);
+ segment.setGoalHandle(options.rt_goal_handle);
+ if (has_rt_goal_handle) {segment.setTolerances(tolerances_per_joint);}
+ result_traj_per_joint.push_back(segment);
+ ++it;
+ }
- // Add useful segments of new trajectory to result
- // - Construct all trajectory segments occurring after current time
- // - As long as there remain two trajectory points we can construct the next trajectory segment
- while (std::distance(it, msg.points.end()) >= 2)
- {
- std::vector::const_iterator next_it = it; ++next_it;
- Segment segment(o_msg_start_time, *it, *next_it, permutation_vector, position_offset);
- segment.setGoalHandle(options.rt_goal_handle);
- if (has_rt_goal_handle) {segment.setTolerances(tolerances);}
- result_traj.push_back(segment);
- ++it;
+ // Useful debug info
+ std::stringstream log_str;
+ log_str << "Trajectory of joint " << joint_names[joint_id] << "has " << result_traj_per_joint.size() << " segments";
+ if (has_current_trajectory)
+ {
+ log_str << ":";
+ log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory.";
+ log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message.";
+ if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) <<
+ " points) taken from the input trajectory.";}
+ }
+ else {log_str << ".";}
+ ROS_DEBUG_STREAM(log_str.str());
+
+ if (result_traj_per_joint.size() > 0)
+ result_traj[joint_id] = result_traj_per_joint;
}
- // Useful debug info
- std::stringstream log_str;
- log_str << "Trajectory has " << result_traj.size() << " segments";
- if (has_current_trajectory)
+ // If the trajectory for all joints is empty, empty the trajectory vector
+ typename Trajectory::const_iterator trajIter = std::find_if (result_traj.begin(), result_traj.end(), isNotEmpty);
+ if (trajIter == result_traj.end())
{
- log_str << ":";
- log_str << "\n- " << num_old_segments << " segment(s) will still be executed from previous trajectory.";
- log_str << "\n- 1 segment added for transitioning between the current trajectory and first point of the input message.";
- if (num_new_segments > 0) {log_str << "\n- " << num_new_segments << " new segments (" << (num_new_segments + 1) <<
- " points) taken from the input trajectory.";}
+ result_traj.clear();
}
- else {log_str << ".";}
- ROS_DEBUG_STREAM(log_str.str());
return result_traj;
}
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
index 4aaf29116..7b5f469d2 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
@@ -40,6 +40,7 @@
// Boost
#include
#include
+#include
// ROS
#include
@@ -166,8 +167,10 @@ class JointTrajectoryController : public controller_interface::Controller StatePublisherPtr;
typedef JointTrajectorySegment Segment;
- typedef std::vector Trajectory;
+ typedef std::vector TrajectoryPerJoint;
+ typedef std::vector Trajectory;
typedef boost::shared_ptr TrajectoryPtr;
+ typedef boost::shared_ptr TrajectoryPerJointPtr;
typedef realtime_tools::RealtimeBox TrajectoryBox;
typedef typename Segment::Scalar Scalar;
@@ -194,11 +197,11 @@ class JointTrajectoryController : public controller_interface::Controller time_data_;
@@ -206,6 +209,8 @@ class JointTrajectoryController : public controller_interface::Controller successful_joint_traj_;
+ bool allow_partial_joints_goal_;
// ROS API
ros::NodeHandle controller_nh_;
@@ -241,33 +246,6 @@ class JointTrajectoryController : public controller_interface::Controller
-inline void JointTrajectoryController::
-checkPathTolerances(const typename Segment::State& state_error,
- const Segment& segment)
-{
- const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
- const SegmentTolerances& tolerances = segment.getTolerances();
- if (!checkStateTolerance(state_error, tolerances.state_tolerance))
- {
- rt_segment_goal->preallocated_result_->error_code =
- control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
- rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
- rt_active_goal_.reset();
- }
-}
-
-template
-inline void JointTrajectoryController::
-checkGoalTolerances(const typename Segment::State& state_error,
- const Segment& segment)
-{
- // Controller uptime
- const ros::Time uptime = time_data_.readFromRT()->uptime;
-
- // Checks that we have ended inside the goal tolerances
- const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
- const SegmentTolerances& tolerances = segment.getTolerances();
- const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance);
-
- if (inside_goal_tolerances)
- {
- rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
- rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
- rt_active_goal_.reset();
- }
- else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance)
- {
- // Still have some time left to meet the goal state tolerances
- }
- else
- {
- if (verbose_)
- {
- ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed");
- // Check the tolerances one more time to output the errors that occures
- checkStateTolerance(state_error, tolerances.goal_state_tolerance, true);
- }
-
- rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
- rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
- rt_active_goal_.reset();
- }
-}
-
template
JointTrajectoryController::
JointTrajectoryController()
@@ -276,6 +222,13 @@ bool JointTrajectoryController::init(HardwareInt
}
ROS_DEBUG_STREAM_NAMED(name_, "Stop trajectory has a duration of " << stop_trajectory_duration_ << "s.");
+ // Checking if partial trajectories are allowed
+ controller_nh_.param("allow_partial_joints_goal", allow_partial_joints_goal_, false);
+ if (allow_partial_joints_goal_)
+ {
+ ROS_DEBUG_NAMED(name_, "Goals with partial set of joints are allowed");
+ }
+
// List of controlled joints
joint_names_ = getStrings(controller_nh_, "joints");
if (joint_names_.empty()) {return false;}
@@ -343,14 +296,26 @@ bool JointTrajectoryController::init(HardwareInt
this);
// Preeallocate resources
- current_state_ = typename Segment::State(n_joints);
- desired_state_ = typename Segment::State(n_joints);
- state_error_ = typename Segment::State(n_joints);
- hold_start_state_ = typename Segment::State(n_joints);
- hold_end_state_ = typename Segment::State(n_joints);
+ current_state_ = typename Segment::State(n_joints);
+ desired_state_ = typename Segment::State(n_joints);
+ state_error_ = typename Segment::State(n_joints);
+ desired_joint_state_ = typename Segment::State(1);
+ state_joint_error_ = typename Segment::State(1);
- Segment hold_segment(0.0, current_state_, 0.0, current_state_);
- hold_trajectory_ptr_->resize(1, hold_segment);
+ successful_joint_traj_ = boost::dynamic_bitset<>(joints_.size());
+
+ // Initialize trajectory with all joints
+ typename Segment::State current_joint_state_ = typename Segment::State(1);
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ current_joint_state_.position[0]= current_state_.position[i];
+ current_joint_state_.velocity[0]= current_state_.velocity[i];
+ Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_);
+
+ TrajectoryPerJoint joint_segment;
+ joint_segment.resize(1, hold_segment);
+ hold_trajectory_ptr_->push_back(joint_segment);
+ }
{
state_publisher_->lock();
@@ -392,16 +357,6 @@ update(const ros::Time& time, const ros::Duration& period)
// fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts in the
// next control cycle, leaving the current cycle without a valid trajectory.
- // Update desired state: sample trajectory at current time
- typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_);
- if (curr_traj.end() == segment_it)
- {
- // Non-realtime safe, but should never happen under normal operation
- ROS_ERROR_NAMED(name_,
- "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
- return;
- }
-
// Update current state and state error
for (unsigned int i = 0; i < joints_.size(); ++i)
{
@@ -409,31 +364,95 @@ update(const ros::Time& time, const ros::Duration& period)
current_state_.velocity[i] = joints_[i].getVelocity();
// There's no acceleration data available in a joint handle
- state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
- state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
+ typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_);
+ if (curr_traj[i].end() == segment_it)
+ {
+ // Non-realtime safe, but should never happen under normal operation
+ ROS_ERROR_NAMED(name_,
+ "Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
+ return;
+ }
+ desired_state_.position[i] = desired_joint_state_.position[0];
+ desired_state_.velocity[i] = desired_joint_state_.velocity[0];
+ desired_state_.acceleration[i] = 0.0;
+
+ state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[i];
+ state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i];
+ state_joint_error_.acceleration[0] = 0.0;
+
+ state_error_.position[i] = desired_joint_state_.position[0] - current_state_.position[i];
+ state_error_.velocity[i] = desired_joint_state_.velocity[0] - current_state_.velocity[i];
state_error_.acceleration[i] = 0.0;
- }
- // Check tolerances if segment corresponds to currently active action goal
- const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
- if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
- {
- // Check tolerances
- if (time_data.uptime.toSec() < segment_it->endTime())
+ //Check tolerances
+ const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
+ if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
{
- // Currently executing a segment: check path tolerances
- checkPathTolerances(state_error_,
- *segment_it);
+ // Check tolerances
+ if (time_data.uptime.toSec() < segment_it->endTime())
+ {
+ // Currently executing a segment: check path tolerances
+ const SegmentTolerancesPerJoint& joint_tolerances = segment_it->getTolerances();
+ if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance))
+ {
+ if (verbose_)
+ {
+ ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]);
+ checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true);
+ }
+ rt_segment_goal->preallocated_result_->error_code =
+ control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
+ rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ successful_joint_traj_.reset();
+ }
+ }
+ else if (segment_it == --curr_traj[i].end())
+ {
+ if (verbose_)
+ ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances");
+
+ // Controller uptime
+ const ros::Time uptime = time_data_.readFromRT()->uptime;
+
+ // Checks that we have ended inside the goal tolerances
+ const SegmentTolerancesPerJoint& tolerances = segment_it->getTolerances();
+ const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance);
+
+ if (inside_goal_tolerances)
+ {
+ successful_joint_traj_[i] = 1;
+ }
+ else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance)
+ {
+ // Still have some time left to meet the goal state tolerances
+ }
+ else
+ {
+ if (verbose_)
+ {
+ ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed for joint: "<< joint_names_[i]);
+ // Check the tolerances one more time to output the errors that occurs
+ checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true);
+ }
+
+ rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
+ rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ successful_joint_traj_.reset();
+ }
+ }
}
- else if (segment_it == --curr_traj.end())
- {
- if (verbose_)
- ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances");
+ }
- // Finished executing the LAST segment: check goal tolerances
- checkGoalTolerances(state_error_,
- *segment_it);
- }
+ //If there is an active goal and all segments finished successfully then set goal as succeeded
+ RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
+ if (current_active_goal and successful_joint_traj_.count() == joints_.size())
+ {
+ current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
+ current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
+ rt_active_goal_.reset();
+ successful_joint_traj_.reset();
}
// Hardware interface adapter: Generate and send commands
@@ -499,12 +518,13 @@ updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePt
curr_trajectory_box_.get(curr_traj_ptr);
Options options;
- options.other_time_base = &next_update_uptime;
- options.current_trajectory = curr_traj_ptr.get();
- options.joint_names = &joint_names_;
- options.angle_wraparound = &angle_wraparound_;
- options.rt_goal_handle = gh;
- options.default_tolerances = &default_tolerances_;
+ options.other_time_base = &next_update_uptime;
+ options.current_trajectory = curr_traj_ptr.get();
+ options.joint_names = &joint_names_;
+ options.angle_wraparound = &angle_wraparound_;
+ options.rt_goal_handle = gh;
+ options.default_tolerances = &default_tolerances_;
+ options.allow_partial_joints_goal = allow_partial_joints_goal_;
// Update currently executing trajectory
try
@@ -539,7 +559,7 @@ template
void JointTrajectoryController::
goalCB(GoalHandle gh)
{
- ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
+ ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal");
// Precondition: Running controller
if (!this->isRunning())
@@ -551,11 +571,24 @@ goalCB(GoalHandle gh)
return;
}
- // Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case
- using internal::permutation;
- std::vector permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);
+ // If partial joints goals are not allowed, goal should specify all controller joints
+ if (!allow_partial_joints_goal_)
+ {
+ if (gh.getGoal()->trajectory.joint_names.size() != joint_names_.size())
+ {
+ ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
+ control_msgs::FollowJointTrajectoryResult result;
+ result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
+ gh.setRejected(result);
+ return;
+ }
+ }
- if (permutation_vector.empty())
+ // Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case
+ using internal::mapping;
+ std::vector mapping_vector = mapping(gh.getGoal()->trajectory.joint_names, joint_names_);
+
+ if (mapping_vector.empty())
{
ROS_ERROR_NAMED(name_, "Joints on incoming goal don't match the controller joints.");
control_msgs::FollowJointTrajectoryResult result;
@@ -638,19 +671,28 @@ queryStateService(control_msgs::QueryTrajectoryState::Request& req,
curr_trajectory_box_.get(curr_traj_ptr);
Trajectory& curr_traj = *curr_traj_ptr;
- typename Segment::State state;
- typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state);
- if (curr_traj.end() == segment_it)
+ typename Segment::State response_point = typename Segment::State(joint_names_.size());
+
+ for (unsigned int i = 0; i < joints_.size(); ++i)
{
- ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
- return false;
- }
+ typename Segment::State state;
+ typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state);
+ if (curr_traj[i].end() == segment_it)
+ {
+ ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time.");
+ return false;
+ }
+ response_point.position[i] = state.position[0];
+ response_point.velocity[i] = state.velocity[0];
+ response_point.acceleration[i] = state.acceleration[0];
+ }
+
// Populate response
resp.name = joint_names_;
- resp.position = state.position;
- resp.velocity = state.velocity;
- resp.acceleration = state.acceleration;
+ resp.position = response_point.position;
+ resp.velocity = response_point.velocity;
+ resp.acceleration = response_point.acceleration;
return true;
}
@@ -690,7 +732,10 @@ setHoldPosition(const ros::Time& time)
// - Create segment that goes from current state to above zero velocity state, in the desired time
// NOTE: The symmetry assumption from the second point above might not hold for all possible segment types
- assert(1 == hold_trajectory_ptr_->size());
+ assert(joint_names_.size() == hold_trajectory_ptr_->size());
+
+ typename Segment::State hold_start_state_ = typename Segment::State(1);
+ typename Segment::State hold_end_state_ = typename Segment::State(1);
const typename Segment::Time start_time = time.toSec();
const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_;
@@ -700,24 +745,24 @@ setHoldPosition(const ros::Time& time)
const unsigned int n_joints = joints_.size();
for (unsigned int i = 0; i < n_joints; ++i)
{
- hold_start_state_.position[i] = joints_[i].getPosition();
- hold_start_state_.velocity[i] = joints_[i].getVelocity();
- hold_start_state_.acceleration[i] = 0.0;
+ hold_start_state_.position[0] = joints_[i].getPosition();
+ hold_start_state_.velocity[0] = joints_[i].getVelocity();
+ hold_start_state_.acceleration[0] = 0.0;
- hold_end_state_.position[i] = joints_[i].getPosition();
- hold_end_state_.velocity[i] = -joints_[i].getVelocity();
- hold_end_state_.acceleration[i] = 0.0;
- }
- hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
- end_time_2x, hold_end_state_);
+ hold_end_state_.position[0] = joints_[i].getPosition();
+ hold_end_state_.velocity[0] = -joints_[i].getVelocity();
+ hold_end_state_.acceleration[0] = 0.0;
- // Sample segment at its midpoint, that should have zero velocity
- hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);
+ (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
+ end_time_2x, hold_end_state_);
- // Now create segment that goes from current state to one with zero end velocity
- hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
- end_time, hold_end_state_);
+ // Sample segment at its midpoint, that should have zero velocity
+ (*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);
+ // Now create segment that goes from current state to one with zero end velocity
+ (*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
+ end_time, hold_end_state_);
+ }
curr_trajectory_box_.set(hold_trajectory_ptr_);
}
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
index 3c024c7cf..6915a7950 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h
@@ -85,31 +85,18 @@ class JointTrajectorySegment : public Segment
/**
* \param point Trajectory point.
*
- * \param permutation Permutation vector for mapping the joint order of a \p point to a desired order.
- * For instance, if \p point contains data associated to joints "{B, D, A, C}", and we are interested in
- * constructing a segment with joints ordered as "{A, B, C, D}", the permutation vector should
- * be set to "{2, 0, 3, 1}".
- * If unspecified (empty), the joint order of \p point is preserved; if specified, its size must coincide with that
- * of \p point.
- * This vector can be computed using the \ref trajectory_interface::internal::permutation()
- * "permutation" utility function.
- *
- * \param position_offset Position offset to applpy to the data in \p point. This parameter is useful for handling
+ * \param position_offset Position offset to apply to the data in \p point. This parameter is useful for handling
* joints that wrap around (ie. continuous), to compensate for multi-turn offsets.
* If unspecified (empty), zero offsets are applied; if specified, its size must coincide with that of \p point.
*
- * \note The offsets in \p position_offsets correspond to joints not ordered according to \p point, but to joints
- * in the expected order, that is \p point with \p permutation applied to it.
*/
State(const trajectory_msgs::JointTrajectoryPoint& point,
- const std::vector& permutation = std::vector(),
const std::vector& position_offset = std::vector())
{
- init(point, permutation, position_offset);
+ init(point, position_offset);
}
void init(const trajectory_msgs::JointTrajectoryPoint& point,
- const std::vector& permutation = std::vector(),
const std::vector& position_offset = std::vector())
{
using std::invalid_argument;
@@ -121,17 +108,6 @@ class JointTrajectorySegment : public Segment
{
throw(invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
}
- if (!permutation.empty() && joint_dim != permutation.size())
- {
- throw(invalid_argument("Size mismatch between trajectory point and permutation vector."));
- }
- for (unsigned int i = 0; i < permutation.size(); ++i)
- {
- if (permutation[i] >= joint_dim)
- {
- throw(invalid_argument("Permutation vector contains out-of-range indices."));
- }
- }
if (!position_offset.empty() && joint_dim != position_offset.size())
{
throw(invalid_argument("Size mismatch between trajectory point "
@@ -145,15 +121,12 @@ class JointTrajectorySegment : public Segment
for (unsigned int i = 0; i < joint_dim; ++i)
{
- // Apply permutation only if it was specified, otherwise preserve original message order
- const unsigned int id = permutation.empty() ? i : permutation[i];
-
// Apply position offset only if it was specified
const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i];
- if (!point.positions.empty()) {this->position[i] = point.positions[id] + offset;}
- if (!point.velocities.empty()) {this->velocity[i] = point.velocities[id];}
- if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[id];}
+ if (!point.positions.empty()) {this->position[i] = point.positions[i] + offset;}
+ if (!point.velocities.empty()) {this->velocity[i] = point.velocities[i];}
+ if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[i];}
}
}
};
@@ -171,7 +144,7 @@ class JointTrajectorySegment : public Segment
const Time& end_time,
const State& end_state)
: rt_goal_handle_(),
- tolerances_(start_state.position.size())
+ tolerances_()
{
Segment::init(start_time, start_state, end_time, end_state);
}
@@ -183,7 +156,6 @@ class JointTrajectorySegment : public Segment
* segment start time.
* \param start_point Start state in ROS message format.
* \param end_point End state in ROS message format.
- * \param permutation See \ref JointTrajectorySegment::State.
* \param position_offset See \ref JointTrajectorySegment::State.
*
* \throw std::invalid_argument If input parameters are inconsistent and a valid segment can't be constructed.
@@ -191,10 +163,9 @@ class JointTrajectorySegment : public Segment
JointTrajectorySegment(const ros::Time& traj_start_time,
const trajectory_msgs::JointTrajectoryPoint& start_point,
const trajectory_msgs::JointTrajectoryPoint& end_point,
- const std::vector& permutation = std::vector(),
const std::vector& position_offset = std::vector())
: rt_goal_handle_(),
- tolerances_(start_point.positions.size())
+ tolerances_()
{
if (start_point.positions.size() != end_point.positions.size())
{
@@ -207,8 +178,8 @@ class JointTrajectorySegment : public Segment
try
{
- const State start_state(start_point, permutation, position_offset);
- const State end_state(end_point, permutation, position_offset);
+ const State start_state(start_point, position_offset);
+ const State end_state(end_point, position_offset);
this->init(start_time, start_state,
end_time, end_state);
@@ -227,51 +198,45 @@ class JointTrajectorySegment : public Segment
void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle) {rt_goal_handle_ = rt_goal_handle;}
/** \return Tolerances this segment is associated to. */
- const SegmentTolerances& getTolerances() const {return tolerances_;}
+ const SegmentTolerancesPerJoint& getTolerances() const {return tolerances_;}
/** \brief Set the tolerances this segment is associated to. */
- void setTolerances(const SegmentTolerances& tolerances) {tolerances_ = tolerances;}
+ void setTolerances(const SegmentTolerancesPerJoint& tolerances) {tolerances_ = tolerances;}
private:
RealtimeGoalHandlePtr rt_goal_handle_;
- SegmentTolerances tolerances_;
+ SegmentTolerancesPerJoint tolerances_;
};
/**
* \param prev_position Previous position from which to compute the wraparound offset.
* \param next_position Next position from which to compute the wraparound offset.
- * \param angle_wraparound Vector of booleans where true values correspond to joints that wrap around
- * (ie. are continuous). Offsets will be computed only for these joints, otherwise they are set to zero.
- * \return Wraparound offsets that should be applied to \p next_position such that no multi-turns are performed when
+ * \param angle_wraparound Boolean where true value corresponds to a joint that wrap around
+ * (ie. is continuous). Offset will be computed only for this joint, otherwise it is set to zero.
+ * \return Wraparound offset that should be applied to \p next_position such that no multi-turns are performed when
* transitioning from \p prev_position.
* \tparam Scalar Scalar type.
*/
template
-std::vector wraparoundOffset(const std::vector& prev_position,
- const std::vector& next_position,
- const std::vector& angle_wraparound)
+Scalar wraparoundJointOffset(const Scalar& prev_position,
+ const Scalar& next_position,
+ const bool& angle_wraparound)
{
- // Preconditions
- const unsigned int n_joints = angle_wraparound.size();
- if (n_joints != prev_position.size() || n_joints != next_position.size()) {return std::vector();}
-
// Return value
- std::vector pos_offset(n_joints, 0.0);
+ Scalar pos_offset = 0.0;
- for (unsigned int i = 0; i < angle_wraparound.size(); ++i)
+ if (angle_wraparound)
{
- if (angle_wraparound[i])
- {
- Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]);
+ Scalar dist = angles::shortest_angular_distance(prev_position, next_position);
- // Deal with singularity at M_PI shortest distance
- if (std::abs(dist) - M_PI < 1e-9)
- {
- dist = next_position[i] > prev_position[i] ? std::abs(dist) : -std::abs(dist);
- }
- pos_offset[i] = (prev_position[i] + dist) - next_position[i];
+ // Deal with singularity at M_PI shortest distance
+ if (std::abs(dist) - M_PI < 1e-9)
+ {
+ dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist);
}
+ pos_offset = (prev_position + dist) - next_position;
}
+
return pos_offset;
}
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
index efc06580a..02e03e0f4 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.h
@@ -88,6 +88,28 @@ struct SegmentTolerances
Scalar goal_time_tolerance;
};
+/**
+ * \brief Trajectory segment tolerances per Joint
+ */
+template
+struct SegmentTolerancesPerJoint
+{
+ SegmentTolerancesPerJoint()
+ : state_tolerance(static_cast(0.0)),
+ goal_state_tolerance(static_cast(0.0)),
+ goal_time_tolerance(static_cast(0.0))
+ {}
+
+ /** State tolerances that appply during segment execution. */
+ StateTolerances state_tolerance;
+
+ /** State tolerances that apply for the goal state only.*/
+ StateTolerances goal_state_tolerance;
+
+ /** Extra time after the segment end time allowed to reach the goal state tolerances. */
+ Scalar goal_time_tolerance;
+};
+
/**
* \param state_error State error to check.
* \param state_tolerance State tolerances to check \p state_error against.
@@ -136,6 +158,45 @@ inline bool checkStateTolerance(const State&
return true;
}
+/**
+ * \param state_error State error to check.
+ * \param state_tolerance State tolerances to check \p state_error against.
+ * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true
+ * \return True if \p state_error fulfills \p state_tolerance.
+ */
+template
+inline bool checkStateTolerancePerJoint(const State& state_error,
+ const StateTolerances& state_tolerance,
+ bool show_errors = false)
+{
+
+ using std::abs;
+
+ const bool is_valid = !(state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position) &&
+ !(state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity) &&
+ !(state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration);
+
+ if (!is_valid)
+ {
+ if( show_errors )
+ {
+ ROS_ERROR_STREAM_NAMED("tolerances","Path state tolerances failed:");
+
+ if (state_tolerance.position > 0.0 && abs(state_error.position[0]) > state_tolerance.position)
+ ROS_ERROR_STREAM_NAMED("tolerances","Position Error: " << state_error.position[0] <<
+ " Position Tolerance: " << state_tolerance.position);
+ if (state_tolerance.velocity > 0.0 && abs(state_error.velocity[0]) > state_tolerance.velocity)
+ ROS_ERROR_STREAM_NAMED("tolerances","Velocity Error: " << state_error.velocity[0] <<
+ " Velocity Tolerance: " << state_tolerance.velocity);
+ if (state_tolerance.acceleration > 0.0 && abs(state_error.acceleration[0]) > state_tolerance.acceleration)
+ ROS_ERROR_STREAM_NAMED("tolerances","Acceleration Error: " << state_error.acceleration[0] <<
+ " Acceleration Tolerance: " << state_tolerance.acceleration);
+ }
+ return false;
+ }
+ return true;
+}
+
/**
* \brief Update data in \p tols from data in \p msg_tol.
*
diff --git a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
index b26985dcf..c72035c1d 100644
--- a/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
+++ b/joint_trajectory_controller/test/init_joint_trajectory_test.cpp
@@ -45,61 +45,73 @@ using std::string;
const double EPS = 1e-9;
typedef JointTrajectorySegment > Segment;
-typedef vector Trajectory;
+typedef vector TrajectoryPerJoint;
+typedef vector Trajectory;
-TEST(PermutationTest, Permutation)
+TEST(MappingTest, Mapping)
{
vector t1(4);
- t1[0] = "A";
- t1[1] = "B";
- t1[2] = "C";
- t1[3] = "D";
+ t1[0] = "B";
+ t1[1] = "D";
+ t1[2] = "A";
+ t1[3] = "C";
vector t2(4);
- t2[0] = "B";
- t2[1] = "D";
- t2[2] = "A";
- t2[3] = "C";
+ t2[0] = "A";
+ t2[1] = "B";
+ t2[2] = "C";
+ t2[3] = "D";
- typedef vector PermutationType;
- PermutationType ground_truth(4);
+ typedef vector MappingType;
+ MappingType ground_truth(4);
ground_truth[0] = 2;
ground_truth[1] = 0;
ground_truth[2] = 3;
ground_truth[3] = 1;
- // Mismatching sizes: Return empty permutation vector
+ // Mismatching sizes (t2 smaller than t1): Return empty mapping vector
{
vector t2_bad(1,"A");
- PermutationType perm = internal::permutation(t1, t2_bad);
- EXPECT_TRUE(perm.empty());
+ MappingType mapping = internal::mapping(t1, t2_bad);
+ EXPECT_TRUE(mapping.empty());
}
- // Mismatching contents: Return empty permutation vector
+ // Mismatching contents: Return empty mapping vector
{
vector t2_bad(3,"A");
- PermutationType perm = internal::permutation(t1, t2_bad);
- EXPECT_TRUE(perm.empty());
+ MappingType mapping = internal::mapping(t1, t2_bad);
+ EXPECT_TRUE(mapping.empty());
}
// Valid parameters
{
- PermutationType perm = internal::permutation(t1, t2);
- EXPECT_EQ(ground_truth.size(), perm.size());
- EXPECT_EQ(ground_truth[0], perm[0]);
- EXPECT_EQ(ground_truth[1], perm[1]);
- EXPECT_EQ(ground_truth[2], perm[2]);
- EXPECT_EQ(ground_truth[3], perm[3]);
+ MappingType mapping = internal::mapping(t1, t2);
+ EXPECT_EQ(ground_truth.size(), mapping.size());
+ EXPECT_EQ(ground_truth[3], mapping[0]);
+ EXPECT_EQ(ground_truth[2], mapping[1]);
+ EXPECT_EQ(ground_truth[1], mapping[2]);
+ EXPECT_EQ(ground_truth[0], mapping[3]);
}
- // Valid parameters, inverse parameter order yields inverse permutation vector
+ // Valid parameters, inverse parameter order yields inverse mapping vector
{
- PermutationType perm = internal::permutation(t2, t1);
- EXPECT_EQ(ground_truth.size(), perm.size());
- EXPECT_EQ(ground_truth[3], perm[0]);
- EXPECT_EQ(ground_truth[2], perm[1]);
- EXPECT_EQ(ground_truth[1], perm[2]);
- EXPECT_EQ(ground_truth[0], perm[3]);
+ MappingType mapping = internal::mapping(t2, t1);
+ EXPECT_EQ(ground_truth.size(), mapping.size());
+ EXPECT_EQ(ground_truth[0], mapping[0]);
+ EXPECT_EQ(ground_truth[1], mapping[1]);
+ EXPECT_EQ(ground_truth[2], mapping[2]);
+ EXPECT_EQ(ground_truth[3], mapping[3]);
+ }
+
+ // Valid parameters, partial trajectory
+ {
+ vector t1_partial(2);
+ t1_partial[0] = "B";
+ t1_partial[1] = "D";
+ MappingType mapping = internal::mapping(t1_partial, t2);
+ EXPECT_EQ(t1_partial.size(), mapping.size());
+ EXPECT_EQ(1, mapping[0]);
+ EXPECT_EQ(3, mapping[1]);
}
}
@@ -140,11 +152,15 @@ class InitTrajectoryTest : public ::testing::Test
state[1].velocity.push_back(1.0);
state[1].acceleration.push_back(1.0);
- curr_traj.push_back(Segment(0.0, state[0],
+ TrajectoryPerJoint joint_segment;
+
+ joint_segment.push_back(Segment(0.0, state[0],
1.0, state[1]));
- curr_traj.push_back(Segment(3.0, state[1],
+ joint_segment.push_back(Segment(3.0, state[1],
10.0, state[0])); // This is just junk that should be discarded in the tests
+
+ curr_traj.push_back(joint_segment);
}
protected:
@@ -182,14 +198,14 @@ TEST_F(InitTrajectoryTest, InitLogic)
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time);
- EXPECT_EQ(points.size() - 1, trajectory.size());
+ EXPECT_EQ(points.size() - 1, trajectory[0].size());
}
// First point: Return partial trajectory (last segment)
{
const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time);
- EXPECT_EQ(points.size() - 2, trajectory.size());
+ EXPECT_EQ(points.size() - 2, trajectory[0].size());
}
// Between the first and second points: Return partial trajectory (last segment)
@@ -197,7 +213,7 @@ TEST_F(InitTrajectoryTest, InitLogic)
const ros::Time time = msg_start_time +
ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory(trajectory_msg, time);
- EXPECT_EQ(msg_points.size() - 2, trajectory.size());
+ EXPECT_EQ(msg_points.size() - 2, trajectory[0].size());
}
// Second point: Return empty trajectory
@@ -249,23 +265,23 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
// Before first point, starting the current trajectory: Return 4 segments: Last of current + bridge + full message
{
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(points.size() + 1, trajectory.size());
+ EXPECT_EQ(points.size() + 1, trajectory[0].size());
}
// Before first point: Return 4 segments: Last of current + bridge + full message
{
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(points.size() + 1, trajectory.size());
+ EXPECT_EQ(points.size() + 1, trajectory[0].size());
}
// First point: Return partial trajectory, 3 segments: Last of current + bridge + last of message
{
const ros::Time time = msg_start_time + msg_points.begin()->time_from_start;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(points.size(), trajectory.size());
+ EXPECT_EQ(points.size(), trajectory[0].size());
}
// Between the first and second points: Same as before
@@ -273,14 +289,14 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
const ros::Time time = msg_start_time +
ros::Duration((msg_points.begin()->time_from_start + (++msg_points.begin())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(msg_points.size(), trajectory.size());
+ EXPECT_EQ(msg_points.size(), trajectory[0].size());
}
// Second point: Return partial trajectory, 2 segments: Last of current + bridge (only one point of message made it)
{
const ros::Time time = msg_start_time + (++msg_points.begin())->time_from_start;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(msg_points.size() - 1, trajectory.size());
+ EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Between the second and third points: Same as before
@@ -288,7 +304,7 @@ TEST_F(InitTrajectoryTest, InitLogicCombine)
const ros::Time time = msg_start_time +
ros::Duration(((++msg_points.begin())->time_from_start + (--msg_points.end())->time_from_start).toSec() / 2.0);
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- EXPECT_EQ(msg_points.size() - 1, trajectory.size());
+ EXPECT_EQ(msg_points.size() - 1, trajectory[0].size());
}
// Last point: Return empty trajectory
@@ -314,12 +330,12 @@ TEST_F(InitTrajectoryTest, InitValues)
// Input time is before first point: Return full trajectory (2 segments)
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time);
- ASSERT_EQ(points.size() - 1, trajectory.size());
+ ASSERT_EQ(points.size() - 1, trajectory[0].size());
// Check all segment start/end times and states
- for (unsigned int i = 0; i < trajectory.size(); ++i)
+ for (unsigned int i = 0; i < trajectory[0].size(); ++i)
{
- const Segment& segment = trajectory[i];
+ const Segment& segment = trajectory[0][i];
const JointTrajectoryPoint& p_start = msg_points[i];
const JointTrajectoryPoint& p_end = msg_points[i + 1];
@@ -354,17 +370,17 @@ TEST_F(InitTrajectoryTest, InitValues)
TEST_F(InitTrajectoryTest, InitValuesCombine)
{
// Before first point: Return 4 segments: Last of current + bridge + full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
InitJointTrajectoryOptions options;
options.current_trajectory = &curr_traj;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- ASSERT_EQ(points.size() + 1, trajectory.size());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[0];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -395,8 +411,8 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
// Check bridge trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[1];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][1];
const vector& msg_points = trajectory_msg.points;
@@ -433,12 +449,12 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
}
// Check all segment start/end times and states (2 segments)
- for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector& msg_points = trajectory_msg.points;
- const Segment& segment = trajectory[traj_it];
+ const Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
@@ -470,7 +486,7 @@ TEST_F(InitTrajectoryTest, InitValuesCombine)
}
}
-TEST_F(InitTrajectoryTest, JointPermutation)
+TEST_F(InitTrajectoryTest, JointMapping)
{
// Add an extra joint to the trajectory message created in the fixture
trajectory_msg.points[0].positions.push_back(-2.0);
@@ -492,11 +508,13 @@ TEST_F(InitTrajectoryTest, JointPermutation)
Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp);
// Check position values only
- const Segment& segment = trajectory[0];
+ const Segment& segment_foo_joint = trajectory[0][0];
+ const Segment& segment_bar_joint = trajectory[1][0];
typename Segment::State state;
- segment.sample(segment.startTime(), state);
+ segment_foo_joint.sample(segment_foo_joint.startTime(), state);
EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[0]);
- EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[1]);
+ segment_bar_joint.sample(segment_bar_joint.startTime(), state);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]);
}
// Reference joint names vector that reverses joint order
@@ -510,13 +528,17 @@ TEST_F(InitTrajectoryTest, JointPermutation)
Trajectory trajectory = initJointTrajectory(trajectory_msg, trajectory_msg.header.stamp, options);
// Check position values only
- const Segment& segment = trajectory[0];
- typename Segment::State state;
- segment.sample(segment.startTime(), state);
- EXPECT_NE(trajectory_msg.points[0].positions[0], state.position[0]);
- EXPECT_NE(trajectory_msg.points[0].positions[1], state.position[1]);
- EXPECT_EQ(trajectory_msg.points[0].positions[0], state.position[1]);
- EXPECT_EQ(trajectory_msg.points[0].positions[1], state.position[0]);
+ const Segment& segment_bar_joint = trajectory[0][0];
+ const Segment& segment_foo_joint = trajectory[1][0];
+
+ typename Segment::State state_foo_joint;
+ typename Segment::State state_bar_joint;
+ segment_foo_joint.sample(segment_foo_joint.startTime(), state_foo_joint);
+ segment_bar_joint.sample(segment_bar_joint.startTime(), state_bar_joint);
+ EXPECT_NE(trajectory_msg.points[0].positions[0], state_bar_joint.position[0]);
+ EXPECT_NE(trajectory_msg.points[0].positions[1], state_foo_joint.position[0]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[0], state_foo_joint.position[0]);
+ EXPECT_EQ(trajectory_msg.points[0].positions[1], state_bar_joint.position[0]);
}
// Reference joint names size mismatch
@@ -553,19 +575,19 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
}
// Before first point: Return 4 segments: Last of current + bridge + full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
std::vector angle_wraparound(1, true);
InitJointTrajectoryOptions options;
options.current_trajectory = &curr_traj;
options.angle_wraparound = &angle_wraparound;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- ASSERT_EQ(points.size() + 1, trajectory.size());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[0];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -596,8 +618,8 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
// Check bridge trajectory segment start/end times and states (only one)
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[1];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][1];
const vector& msg_points = trajectory_msg.points;
@@ -634,12 +656,12 @@ TEST_F(InitTrajectoryTest, WrappingSpec)
}
// Check all segment start/end times and states (2 segments)
- for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector& msg_points = trajectory_msg.points;
- const Segment& segment = trajectory[traj_it];
+ const Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
@@ -696,18 +718,18 @@ TEST_F(InitTrajectoryTest, IgnoreWrappingSpec)
}
// Before first point: Return 2 segments: Full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
std::vector angle_wraparound(1, true);
InitJointTrajectoryOptions options;
options.angle_wraparound = &angle_wraparound;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- ASSERT_EQ(points.size() - 1, trajectory.size());
+ ASSERT_EQ(points.size() - 1, trajectory[0].size());
// Check all segment start/end times and states (2 segments)
- for (unsigned int i = 0; i < trajectory.size(); ++i)
+ for (unsigned int i = 0; i < trajectory[0].size(); ++i)
{
- const Segment& segment = trajectory[i];
+ const Segment& segment = trajectory[0][i];
const JointTrajectoryPoint& p_start = msg_points[i];
const JointTrajectoryPoint& p_end = msg_points[i + 1];
@@ -751,21 +773,21 @@ TEST_F(InitTrajectoryTest, GoalHandleTest)
RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
// Before first point: Return 4 segments: Last of current + bridge + full message
- const ros::Time time(curr_traj[0].startTime());
+ const ros::Time time(curr_traj[0][0].startTime());
InitJointTrajectoryOptions options;
options.current_trajectory = &curr_traj;
options.rt_goal_handle = rt_goal;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- ASSERT_EQ(points.size() + 1, trajectory.size());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Segment of current trajectory preserves existing null goal handle
- ASSERT_FALSE(trajectory[0].getGoalHandle());
+ ASSERT_FALSE(trajectory[0][0].getGoalHandle());
// All further segments should be associated to the new goal handle
- for (unsigned int i = 1; i < trajectory.size(); ++i) // NOTE: Start index != 0
+ for (unsigned int i = 1; i < trajectory[0].size(); ++i) // NOTE: Start index != 0
{
- ASSERT_EQ(rt_goal, trajectory[i].getGoalHandle());
+ ASSERT_EQ(rt_goal, trajectory[0][i].getGoalHandle());
}
}
@@ -783,12 +805,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
// Before first point: Return 4 segments: Last of current + bridge + full message
const ros::Time time = msg_start_time;
Trajectory trajectory = initJointTrajectory(trajectory_msg, time, options);
- ASSERT_EQ(points.size() + 1, trajectory.size());
+ ASSERT_EQ(points.size() + 1, trajectory[0].size());
// Check current trajectory segment start/end times (only one). Time offset does NOT apply
{
- const Segment& ref_segment = curr_traj[0];
- const Segment& segment = trajectory[0];
+ const Segment& ref_segment = curr_traj[0][0];
+ const Segment& segment = trajectory[0][0];
// Check start/end times
{
@@ -799,7 +821,7 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
// Check bridge trajectory segment start/end times and states (only one). Time offset applies to end state only
{
- const Segment& segment = trajectory[1];
+ const Segment& segment = trajectory[0][1];
const vector& msg_points = trajectory_msg.points;
@@ -817,12 +839,12 @@ TEST_F(InitTrajectoryTest, OtherTimeBase)
}
// Check all segment start/end times and states (2 segments). Time offset does apply
- for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory.size(); ++traj_it, ++msg_it)
+ for (unsigned int traj_it = 2, msg_it = 0; traj_it < trajectory[0].size(); ++traj_it, ++msg_it)
{
const ros::Time msg_start_time = trajectory_msg.header.stamp;
const vector& msg_points = trajectory_msg.points;
- const Segment& segment = trajectory[traj_it];
+ const Segment& segment = trajectory[0][traj_it];
const JointTrajectoryPoint& p_start = msg_points[msg_it];
const JointTrajectoryPoint& p_end = msg_points[msg_it + 1];
diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller.test b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test
new file mode 100644
index 000000000..7962df69c
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller.test
@@ -0,0 +1,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp
new file mode 100644
index 000000000..ad45a717d
--- /dev/null
+++ b/joint_trajectory_controller/test/joint_partial_trajectory_controller_test.cpp
@@ -0,0 +1,637 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2016, Shadow Robot Company Ltd.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Beatriz Leon
+
+#include
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+// Floating-point value comparison threshold
+const double EPS = 0.01;
+
+using actionlib::SimpleClientGoalState;
+
+class JointPartialTrajectoryControllerTest : public ::testing::Test
+{
+public:
+ JointPartialTrajectoryControllerTest()
+ : nh("rrbot_controller"),
+ short_timeout(1.0),
+ long_timeout(10.0),
+ controller_state()
+ {
+ n_joints = (2);
+ n_partial_joints = (2);
+ joint_names.resize(n_joints);
+ joint_names[0] = "joint1";
+ joint_names[1] = "joint2";
+
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.positions.resize(n_joints, 0.0);
+ point.velocities.resize(n_joints, 0.0);
+ point.accelerations.resize(n_joints, 0.0);
+
+ // Go home trajectory
+ traj_home.joint_names = joint_names;
+ traj_home.points.resize(1, point);
+ traj_home.points[0].time_from_start = ros::Duration(1.0);
+
+ // Three-point trajectory
+ points.resize(3, point);
+ points[0].positions[0] = M_PI / 4.0;
+ points[0].positions[1] = 0.0;
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions[0] = 0.0;
+ points[1].positions[1] = -M_PI / 4.0;
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions[0] = -M_PI / 4.0;
+ points[2].positions[1] = M_PI / 4.0;
+ points[2].time_from_start = ros::Duration(4.0);
+
+ traj.joint_names = joint_names;
+ traj.points = points;
+
+ // Partial trajectory
+ traj_partial.joint_names.resize(1, "joint2");
+ points.resize(3, point);
+ points[0].positions[0] = M_PI / 4.0;
+ points[0].time_from_start = ros::Duration(1.0);
+
+ points[1].positions[0] = M_PI / 2.0;
+ points[1].time_from_start = ros::Duration(2.0);
+
+ points[2].positions[0] = -M_PI / 2.0;
+ points[2].time_from_start = ros::Duration(4.0);
+ traj_partial.points = points;
+
+ // Action goals
+ traj_home_goal.trajectory = traj_home;
+ traj_goal.trajectory = traj;
+ traj_partial_goal.trajectory = traj_partial;
+
+ // Smoothing publisher (determines how well the robot follows a trajectory)
+ smoothing_pub = ros::NodeHandle().advertise("smoothing", 1);
+
+ // Trajectory publisher
+ traj_pub = nh.advertise("command", 1);
+
+ // State subscriber
+ state_sub = nh.subscribe("state",
+ 1,
+ &JointPartialTrajectoryControllerTest::stateCB,
+ this);
+
+ // Query state service client
+ query_state_service = nh.serviceClient("query_state");
+
+ // Action client
+ const std::string action_server_name = nh.getNamespace() + "/follow_joint_trajectory";
+ action_client.reset(new ActionClient(action_server_name));
+ action_client2.reset(new ActionClient(action_server_name));
+ }
+
+ ~JointPartialTrajectoryControllerTest()
+ {
+ state_sub.shutdown(); // This is important, to make sure that the callback is not woken up later in the destructor
+ }
+
+protected:
+ typedef actionlib::SimpleActionClient ActionClient;
+ typedef boost::shared_ptr ActionClientPtr;
+ typedef control_msgs::FollowJointTrajectoryGoal ActionGoal;
+ typedef control_msgs::JointTrajectoryControllerStateConstPtr StateConstPtr;
+
+ boost::mutex mutex;
+ ros::NodeHandle nh;
+
+ unsigned int n_joints;
+ unsigned int n_partial_joints;
+ std::vector joint_names;
+ std::vector points;
+
+ trajectory_msgs::JointTrajectory traj_home;
+ trajectory_msgs::JointTrajectory traj;
+ trajectory_msgs::JointTrajectory traj_partial;
+ ActionGoal traj_home_goal;
+ ActionGoal traj_goal;
+ ActionGoal traj_partial_goal;
+
+ ros::Duration short_timeout;
+ ros::Duration long_timeout;
+
+ ros::Publisher smoothing_pub;
+ ros::Publisher traj_pub;
+ ros::Subscriber state_sub;
+ ros::ServiceClient query_state_service;
+ ActionClientPtr action_client;
+ ActionClientPtr action_client2;
+
+
+ StateConstPtr controller_state;
+
+ void stateCB(const StateConstPtr& state)
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ controller_state = state;
+ }
+
+ StateConstPtr getState()
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ return controller_state;
+ }
+
+ bool initState(const ros::Duration& timeout = ros::Duration(5.0))
+ {
+ bool init_ok = false;
+ ros::Time start_time = ros::Time::now();
+ while (!init_ok && (ros::Time::now() - start_time) < timeout)
+ {
+ {
+ boost::mutex::scoped_lock lock(mutex);
+ init_ok = controller_state && !controller_state->joint_names.empty();
+ }
+ ros::Duration(0.1).sleep();
+ }
+ return init_ok;
+ }
+
+ static bool waitForState(const ActionClientPtr& action_client,
+ const actionlib::SimpleClientGoalState& state,
+ const ros::Duration& timeout)
+ {
+ using ros::Time;
+ using ros::Duration;
+
+ Time start_time = Time::now();
+ while (action_client->getState() != state && ros::ok())
+ {
+ if (timeout >= Duration(0.0) && (Time::now() - start_time) > timeout) {return false;} // Timed-out
+ ros::Duration(0.01).sleep();
+ }
+ return true;
+ }
+};
+
+// Invalid messages ////////////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, invalidMessages)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Incompatible joint names
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.joint_names[0] = "bad_name";
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS);
+ }
+
+ // No position data
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.points[0].positions.clear();
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Incompatible data sizes
+ {
+ ActionGoal bad_goal = traj_home_goal;
+ bad_goal.trajectory.points[0].positions.pop_back();
+
+ bad_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Non-strictly increasing waypoint times
+ {
+ ActionGoal bad_goal = traj_goal;
+ bad_goal.trajectory.points[2].time_from_start = bad_goal.trajectory.points[1].time_from_start;
+
+ action_client->sendGoal(bad_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_GOAL);
+ }
+
+ // Empty trajectory through action interface
+ // NOTE: Sending an empty trajectory through the topic interface cancels execution of all queued segments, but
+ // an empty trajectory is interpreted by the action interface as not having the correct joint names.
+ {
+ ActionGoal empty_goal;
+ action_client->sendGoal(empty_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::REJECTED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS);
+ }
+}
+
+// Partial trajectory execution //////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, allowPartialTrajIfSpecified)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send partial trajectory.
+ action_client->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, long_timeout));
+}
+
+// Uninterrupted Partial trajectory execution //////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, topicSinglePartialTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send partial trajectory
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+ ros::Duration wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[1], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[1], EPS);
+
+ // Validate query state service
+ control_msgs::QueryTrajectoryState srv;
+ srv.request.time = ros::Time::now();
+ ASSERT_TRUE(query_state_service.call(srv));
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(state->desired.positions[i], srv.response.position[i], EPS);
+ EXPECT_NEAR(state->desired.velocities[i], srv.response.velocity[i], EPS);
+ EXPECT_NEAR(state->desired.accelerations[i], srv.response.acceleration[i], EPS);
+ }
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, actionSinglePartialTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send partial trajectory
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_partial_goal);
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::SUCCESSFUL);
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Validate state topic values
+ StateConstPtr state = getState();
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->actual.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->actual.velocities[1], EPS);
+
+ EXPECT_NEAR(0.0, state->error.positions[1], EPS);
+ EXPECT_NEAR(0.0, state->error.velocities[1], EPS);
+}
+
+// Trajectory replacement //////////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, topicReplacesTopicTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory that preempts the previous one
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+ wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, actionReplacesActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory that preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ //ROS_ERROR_STREAM("state: "<< action_client->getState().toString());
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, actionReplacesTopicTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory that preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, topicReplacesActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory that preempts the previous one
+ traj_partial.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj_partial);
+
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+
+ wait_duration = traj_partial.points.back().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until done
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+// Ignore old trajectory points ////////////////////////////////////////////////////////////////////////////////////////
+
+TEST_F(JointPartialTrajectoryControllerTest, ignoreOldTopicTraj)
+{
+ ASSERT_TRUE(initState());
+
+ // Send trajectory
+ traj.header.stamp = ros::Time(0); // Start immediately
+ traj_pub.publish(traj);
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_partial.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5);
+ traj_pub.publish(traj_partial);
+
+ wait_duration = traj.points.back().time_from_start - traj.points.front().time_from_start + ros::Duration(0.5);
+ wait_duration.sleep(); // Wait until first trajectory is done
+
+ // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial)
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, ignoreOldActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(traj_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = traj.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory with all points occuring in the past. Should not preempts the previous one
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj_partial.points.back().time_from_start - ros::Duration(0.5);
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::REJECTED, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory end (Joint2 has not moved according to traj_partial)
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, ignorePartiallyOldActionTraj)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ ActionGoal first_goal = traj_goal;
+ first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(first_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() - traj.points.back().time_from_start + ros::Duration(1.0);
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ StateConstPtr state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+TEST_F(JointPartialTrajectoryControllerTest, executeParitalActionTrajInFuture)
+{
+ ASSERT_TRUE(initState());
+ ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+ // Send trajectory
+ ActionGoal first_goal = traj_goal;
+ first_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+ action_client->sendGoal(first_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ ros::Duration wait_duration = first_goal.trajectory.points.front().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ // Send partial trajectory with only the last point not occuring in the past. Only the last point should be executed
+ traj_partial_goal.trajectory.header.stamp = ros::Time::now() + traj.points.back().time_from_start + ros::Duration(2.0);
+ action_client2->sendGoal(traj_partial_goal);
+ ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::PREEMPTED, short_timeout));
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::ACTIVE, short_timeout));
+
+ // Wait until first traj is done
+ wait_duration = first_goal.trajectory.points.back().time_from_start;
+ wait_duration.sleep(); // Wait until ~first waypoint
+
+ StateConstPtr state = getState();
+ for (unsigned int i = 0; i < n_joints; ++i)
+ {
+ EXPECT_NEAR(traj.points.back().positions[i], state->desired.positions[i], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[i], state->desired.velocities[i], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[i], state->desired.accelerations[i], EPS);
+ }
+
+ // Wait until done
+ ASSERT_TRUE(waitForState(action_client2, SimpleClientGoalState::SUCCEEDED, long_timeout));
+ ros::Duration(0.5).sleep(); // Allows values to settle to within EPS, especially accelerations
+
+ // Check that we're at the original trajectory for joint1 and on the partial trajectory for joint2
+ state = getState();
+ EXPECT_NEAR(traj.points.back().positions[0], state->desired.positions[0], EPS);
+ EXPECT_NEAR(traj.points.back().velocities[0], state->desired.velocities[0], EPS);
+ EXPECT_NEAR(traj.points.back().accelerations[0], state->desired.accelerations[0], EPS);
+
+ EXPECT_NEAR(traj_partial.points.back().positions[0], state->desired.positions[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().velocities[0], state->desired.velocities[1], EPS);
+ EXPECT_NEAR(traj_partial.points.back().accelerations[0], state->desired.accelerations[1], EPS);
+}
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "joint_partial_trajectory_controller_test");
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ int ret = RUN_ALL_TESTS();
+ spinner.stop();
+ ros::shutdown();
+ return ret;
+}
diff --git a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
index bb357c5ba..7dfd1ea48 100644
--- a/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
+++ b/joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
@@ -248,7 +248,7 @@ TEST_F(JointTrajectoryControllerTest, invalidMessages)
ASSERT_TRUE(initState());
ASSERT_TRUE(action_client->waitForServer(long_timeout));
- // Invalid size
+ // Invalid size (No partial joints goals allowed)
{
trajectory_msgs::JointTrajectoryPoint point;
point.positions.resize(1, 0.0);
diff --git a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
index 546dec449..14ac71cd0 100644
--- a/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
+++ b/joint_trajectory_controller/test/joint_trajectory_segment_test.cpp
@@ -41,7 +41,7 @@ using namespace trajectory_msgs;
const double EPS = 1e-9;
typedef JointTrajectorySegment > Segment;
-typedef std::vector PermutationType;
+typedef std::vector MappingType;
TEST(WraparoundOffsetTest, WrappingPositions)
{
@@ -60,10 +60,10 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// No wrapping joints
{
std::vector angle_wraparound(2, false);
- std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
- EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
// Single wrapping joint
@@ -74,18 +74,18 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// From state1 to state2
{
- std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS);
- EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(-two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
// From state2 to state1
{
- std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(two_pi, wraparound_offset[0], EPS);
- EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
}
@@ -95,18 +95,18 @@ TEST(WraparoundOffsetTest, WrappingPositions)
// From state1 to state2
{
- std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(-two_pi, wraparound_offset[0], EPS);
- EXPECT_NEAR(-2.0 * two_pi, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(-two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(-2.0 * two_pi, wraparound_offset2, EPS);
}
// From state2 to state1
{
- std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(two_pi, wraparound_offset[0], EPS);
- EXPECT_NEAR(2.0 * two_pi, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(two_pi, wraparound_offset1, EPS);
+ EXPECT_NEAR(2.0 * two_pi, wraparound_offset2, EPS);
}
}
}
@@ -122,16 +122,14 @@ TEST(WraparoundOffsetTest, WrappingPositionsPiSingularity)
std::vector angle_wraparound(1, true);
// From state1 to state2
{
- std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
- EXPECT_EQ(pos1.size(), wraparound_offset.size());
- EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
}
// From state2 to state1
{
- std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound);
- EXPECT_EQ(pos1.size(), wraparound_offset.size());
- EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
}
}
@@ -154,18 +152,18 @@ TEST(WraparoundOffsetTest, NonWrappingPositions)
// From state1 to state2
{
- std::vector wraparound_offset = wraparoundOffset(pos1, pos2, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
- EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos1[0], pos2[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos1[1], pos2[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
// From state2 to state1
{
- std::vector wraparound_offset = wraparoundOffset(pos2, pos1, angle_wraparound);
- EXPECT_NEAR(pos1.size(), wraparound_offset.size(), EPS);
- EXPECT_NEAR(0.0, wraparound_offset[0], EPS);
- EXPECT_NEAR(0.0, wraparound_offset[1], EPS);
+ double wraparound_offset1 = wraparoundJointOffset(pos2[0], pos1[0], angle_wraparound[0]);
+ double wraparound_offset2 = wraparoundJointOffset(pos2[1], pos1[1], angle_wraparound[1]);
+ EXPECT_NEAR(0.0, wraparound_offset1, EPS);
+ EXPECT_NEAR(0.0, wraparound_offset2, EPS);
}
}
}
@@ -239,28 +237,11 @@ TEST_F(JointTrajectorySegmentTest, InvalidSegmentConstruction)
catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
}
- // Invalid permutation vector size
- {
- PermutationType permutation(2, 1);
- EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument);
- try {Segment(traj_start_time, p_start, p_end, permutation);}
- catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
- }
-
- // Invalid permutation vector indices
- {
- PermutationType permutation(1, 1);
- EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation), std::invalid_argument);
- try {Segment(traj_start_time, p_start, p_end, permutation);}
- catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
- }
-
// Invalid joint wraparound specification
{
- PermutationType permutation;
std::vector pos_offset(2);
- EXPECT_THROW(Segment(traj_start_time, p_start, p_end, permutation, pos_offset), std::invalid_argument);
- try {Segment(traj_start_time, p_start, p_end, permutation, pos_offset);}
+ EXPECT_THROW(Segment(traj_start_time, p_start, p_end, pos_offset), std::invalid_argument);
+ try {Segment(traj_start_time, p_start, p_end, pos_offset);}
catch (const std::invalid_argument& ex) {ROS_ERROR_STREAM(ex.what());}
}
}
@@ -372,7 +353,7 @@ TEST_F(JointTrajectorySegmentTest, CrossValidateSegmentConstruction)
}
// TODO: Test with dimension four data
-TEST_F(JointTrajectorySegmentTest, PermutationTest)
+TEST_F(JointTrajectorySegmentTest, MappingTest)
{
// Add an extra joint to the trajectory point messages created in the fixture
p_start.positions.push_back(-p_start.positions[0]);
@@ -381,7 +362,7 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest)
p_end.positions.push_back(-p_end.positions[0]);
p_end.velocities.push_back(-p_end.velocities[0]);
- // No permutation vector
+ // No mapping vector
{
// Construct segment from ROS message
EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end));
@@ -393,42 +374,6 @@ TEST_F(JointTrajectorySegmentTest, PermutationTest)
EXPECT_EQ(p_start.positions[0], state.position[0]);
EXPECT_EQ(p_start.positions[1], state.position[1]);
}
-
- // Permutation vector preserving trajectory message joint order
- {
- PermutationType permutation(2);
- permutation[0] = 0;
- permutation[1] = 1;
-
- // Construct segment from ROS message
- EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation));
- Segment segment(traj_start_time, p_start, p_end, permutation);
-
- // Check position values of start state only
- typename Segment::State state;
- segment.sample(segment.startTime(), state);
- EXPECT_EQ(p_start.positions[0], state.position[0]);
- EXPECT_EQ(p_start.positions[1], state.position[1]);
- }
-
- // Permutation vector reversing trajectory message joint order
- {
- PermutationType permutation(2);
- permutation[0] = 1;
- permutation[1] = 0;
-
- // Construct segment from ROS message
- EXPECT_NO_THROW(Segment(traj_start_time, p_start, p_end, permutation));
- Segment segment(traj_start_time, p_start, p_end, permutation);
-
- // Check position values of start state only
- typename Segment::State state;
- segment.sample(segment.startTime(), state);
- EXPECT_NE(p_start.positions[0], state.position[0]);
- EXPECT_NE(p_start.positions[1], state.position[1]);
- EXPECT_EQ(p_start.positions[0], state.position[1]);
- EXPECT_EQ(p_start.positions[1], state.position[0]);
- }
}
int main(int argc, char** argv)
diff --git a/joint_trajectory_controller/test/rrbot_partial_controllers.yaml b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml
new file mode 100644
index 000000000..e7a319544
--- /dev/null
+++ b/joint_trajectory_controller/test/rrbot_partial_controllers.yaml
@@ -0,0 +1,15 @@
+rrbot_controller:
+ type: "position_controllers/JointTrajectoryController"
+ joints:
+ - joint1
+ - joint2
+
+ constraints:
+ goal_time: 0.5
+ joint1:
+ goal: 0.01
+ trajectory: 0.05
+ joint2:
+ goal: 0.01
+ trajectory: 0.05
+ allow_partial_joints_goal: true