JointTrajectoryController: error_string in Result is not populated when the goal is aborted for violated tolerances

[alain-m]

The error_string field in the result of control_msgs/FollowJointTrajectory.action - http://docs.ros.org/api/control_msgs/html/action/FollowJointTrajectory.html - does not seem to get populated when a goal is aborted because of either PATH_TOLERANCE_VIOLATED or GOAL_TOLERANCE_VIOLATED, i.e during

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();
	}
	}
	}
	}

Only the error_code is.
This is contrary to the documentation:

# Human readable description of the error code. Contains complementary
# information that is especially useful when execution fails, for instance:
# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested
#   trajectory is in the past).
# - INVALID_JOINTS: The mismatch between the expected controller joints
#   and those provided in the goal.
# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint
#   violated which tolerance, and by how much.
string error_string

[bmagyar]

We'd be very happy to review a PR on this for ROS Noetic and I can do the necessary backporting and releases for Melodic as well.