JointTrajectoryController: goal time tolerance is only enforced during the last segment of the trajectory

[alain-m]

Reading how state/time tolerances are checked at

ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

Lines 399 to 456 in 12a70ba

	if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
	{
	// Check tolerances
	if (time_data.uptime.toSec() < segment_it->endTime())
	{
	// Currently executing a segment: check path tolerances
	const SegmentTolerancesPerJoint<Scalar>& 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<Scalar>& 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();
	}
	}
	}
	}

it seems that goal_time_tolerance is only used on the last segment of the trajectory - inside else if (segment_it == --curr_traj[i].end()) -.
The goal documentation at http://docs.ros.org/diamondback/api/control_msgs/html/msg/FollowJointTrajectoryGoal.html specifies:

# To report success, the joints must be within goal_tolerance of the
# final trajectory value.  The goal must be achieved by time the
# trajectory ends plus goal_time_tolerance.  (goal_time_tolerance
# allows some leeway in time, so that the trajectory goal can still
# succeed even if the joints reach the goal some time after the
# precise end time of the trajectory).
#
# If the joints are not within goal_tolerance after "trajectory finish
# time" + goal_time_tolerance, the goal aborts with error_code set to
# GOAL_TOLERANCE_VIOLATED

which is correct, but a little unclear in the case where the goal time tolerance is exceed prior to the last segment of the trajectory (e.g the execution hangs because of a hardware failure) then the goal does not get aborted and it will continue to hang indefinitely (unless a timeout was specified for the actionlib goal).
It seems like a more user-friendly behaviour would be to either:

• support individual time tolerances for each segment, and/or
• enforce the (global) goal_time_tolerance on all segments, and not just the last one

[bmagyar]

FYI @ddengster