driver: difference between global AS and per-group AS

[gavanderhoorn]

There is quite a difference between the regular FollowJointTrajectory ActionServer started without any namespacing, and the ones created for each of the motion groups in motoman_driver.

The goal callback for the 'global' one:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp

Lines 192 to 305 in 42e30c4

	void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh)
	{
	gh.setAccepted();
	int group_number;
	// TODO(thiagodefreitas): change for getting the id from the group instead of a sequential checking on the map
	ros::Duration last_time_from_start(0.0);
	motoman_msgs::DynamicJointTrajectory dyn_traj;
	for (int i = 0; i < gh.getGoal()->trajectory.points.size(); i++)
	{
	motoman_msgs::DynamicJointPoint dpoint;
	for (int rbt_idx = 0; rbt_idx < robot_groups_.size(); rbt_idx++)
	{
	size_t ros_idx = std::find(
	gh.getGoal()->trajectory.joint_names.begin(),
	gh.getGoal()->trajectory.joint_names.end(),
	robot_groups_[rbt_idx].get_joint_names()[0])
	- gh.getGoal()->trajectory.joint_names.begin();
	bool is_found = ros_idx < gh.getGoal()->trajectory.joint_names.size();
	group_number = rbt_idx;
	motoman_msgs::DynamicJointsGroup dyn_group;
	int num_joints = robot_groups_[group_number].get_joint_names().size();
	if (is_found)
	{
	if (gh.getGoal()->trajectory.points[i].positions.empty())
	{
	std::vector<double> positions(num_joints, 0.0);
	dyn_group.positions = positions;
	}
	else
	dyn_group.positions.insert(
	dyn_group.positions.begin(),
	gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx,
	gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx
	+ robot_groups_[rbt_idx].get_joint_names().size());
	if (gh.getGoal()->trajectory.points[i].velocities.empty())
	{
	std::vector<double> velocities(num_joints, 0.0);
	dyn_group.velocities = velocities;
	}
	else
	dyn_group.velocities.insert(
	dyn_group.velocities.begin(),
	gh.getGoal()->trajectory.points[i].velocities.begin()
	+ ros_idx, gh.getGoal()->trajectory.points[i].velocities.begin()
	+ ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
	if (gh.getGoal()->trajectory.points[i].accelerations.empty())
	{
	std::vector<double> accelerations(num_joints, 0.0);
	dyn_group.accelerations = accelerations;
	}
	else
	dyn_group.accelerations.insert(
	dyn_group.accelerations.begin(),
	gh.getGoal()->trajectory.points[i].accelerations.begin()
	+ ros_idx, gh.getGoal()->trajectory.points[i].accelerations.begin()
	+ ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
	if (gh.getGoal()->trajectory.points[i].effort.empty())
	{
	std::vector<double> effort(num_joints, 0.0);
	dyn_group.effort = effort;
	}
	else
	dyn_group.effort.insert(
	dyn_group.effort.begin(),
	gh.getGoal()->trajectory.points[i].effort.begin()
	+ ros_idx, gh.getGoal()->trajectory.points[i].effort.begin()
	+ ros_idx + robot_groups_[rbt_idx].get_joint_names().size());
	dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
	dyn_group.group_number = group_number;
	dyn_group.num_joints = dyn_group.positions.size();
	}
	// Generating message for groups that were not present in the trajectory message
	else
	{
	std::vector<double> positions(num_joints, 0.0);
	std::vector<double> velocities(num_joints, 0.0);
	std::vector<double> accelerations(num_joints, 0.0);
	std::vector<double> effort(num_joints, 0.0);
	dyn_group.positions = positions;
	dyn_group.velocities = velocities;
	dyn_group.accelerations = accelerations;
	dyn_group.effort = effort;
	dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
	dyn_group.group_number = group_number;
	dyn_group.num_joints = num_joints;
	}
	dpoint.groups.push_back(dyn_group);
	}
	dpoint.num_groups = dpoint.groups.size();
	dyn_traj.points.push_back(dpoint);
	}
	dyn_traj.header = gh.getGoal()->trajectory.header;
	dyn_traj.header.stamp = ros::Time::now();
	// Publishing the joint names for the 4 groups
	dyn_traj.joint_names = all_joint_names_;
	this->pub_trajectory_command_.publish(dyn_traj);
	}

The callback for the per-group one:

motoman/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp

Lines 315 to 429 in 42e30c4

	void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh, int group_number)
	{
	if (!gh.getGoal()->trajectory.points.empty())
	{
	if (industrial_utils::isSimilar(
	this->robot_groups_[group_number].get_joint_names(),
	gh.getGoal()->trajectory.joint_names))
	{
	// Cancels the currently active goal.
	if (has_active_goal_map_[group_number])
	{
	ROS_WARN("Received new goal, canceling current goal");
	abortGoal(group_number);
	}
	// Sends the trajectory along to the controller
	if (withinGoalConstraints(last_trajectory_state_map_[group_number],
	gh.getGoal()->trajectory, group_number))
	{
	ROS_INFO_STREAM("Already within goal constraints, setting goal succeeded");
	gh.setAccepted();
	gh.setSucceeded();
	has_active_goal_map_[group_number] = false;
	}
	else
	{
	gh.setAccepted();
	active_goal_map_[group_number] = gh;
	has_active_goal_map_[group_number] = true;
	ROS_INFO("Publishing trajectory");
	current_traj_map_[group_number] = active_goal_map_[group_number].getGoal()->trajectory;
	int num_joints = robot_groups_[group_number].get_joint_names().size();
	motoman_msgs::DynamicJointTrajectory dyn_traj;
	for (int i = 0; i < current_traj_map_[group_number].points.size(); ++i)
	{
	motoman_msgs::DynamicJointsGroup dyn_group;
	motoman_msgs::DynamicJointPoint dyn_point;
	if (gh.getGoal()->trajectory.points[i].positions.empty())
	{
	std::vector<double> positions(num_joints, 0.0);
	dyn_group.positions = positions;
	}
	else
	dyn_group.positions = gh.getGoal()->trajectory.points[i].positions;
	if (gh.getGoal()->trajectory.points[i].velocities.empty())
	{
	std::vector<double> velocities(num_joints, 0.0);
	dyn_group.velocities = velocities;
	}
	else
	dyn_group.velocities = gh.getGoal()->trajectory.points[i].velocities;
	if (gh.getGoal()->trajectory.points[i].accelerations.empty())
	{
	std::vector<double> accelerations(num_joints, 0.0);
	dyn_group.accelerations = accelerations;
	}
	else
	dyn_group.accelerations = gh.getGoal()->trajectory.points[i].accelerations;
	if (gh.getGoal()->trajectory.points[i].effort.empty())
	{
	std::vector<double> effort(num_joints, 0.0);
	dyn_group.effort = effort;
	}
	else
	dyn_group.effort = gh.getGoal()->trajectory.points[i].effort;
	dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start;
	dyn_group.group_number = group_number;
	dyn_group.num_joints = robot_groups_[group_number].get_joint_names().size();
	dyn_point.groups.push_back(dyn_group);
	dyn_point.num_groups = 1;
	dyn_traj.points.push_back(dyn_point);
	}
	dyn_traj.header = gh.getGoal()->trajectory.header;
	dyn_traj.joint_names = gh.getGoal()->trajectory.joint_names;
	this->pub_trajectories_[group_number].publish(dyn_traj);
	}
	}
	else
	{
	ROS_ERROR("Joint trajectory action failing on invalid joints");
	control_msgs::FollowJointTrajectoryResult rslt;
	rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
	gh.setRejected(rslt, "Joint names do not match");
	}
	}
	else
	{
	ROS_ERROR("Joint trajectory action failed on empty trajectory");
	control_msgs::FollowJointTrajectoryResult rslt;
	rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
	gh.setRejected(rslt, "Empty trajectory");
	}
	// Adding some informational log messages to indicate unsupported goal constraints
	if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
	{
	ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future");
	}
	if (!gh.getGoal()->goal_tolerance.empty())
	{
	ROS_WARN_STREAM(
	"Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead");
	}
	if (!gh.getGoal()->path_tolerance.empty())
	{
	ROS_WARN_STREAM("Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
	}
	}

Notice the absence of a lot of error checking and handling code, checking for empty goals, joint presence and order checks and other things in the global callback. Initially I assumed this was because the global one was a straight copy of the one in industrial_core (and the rest of the code being the extension for multi-group support), but that doesn't seem to be the case.

This divergence (together with some other missing logic and handling of incoming messages and goals) seems to be the underlying cause for a nr of outstanding issues (among which #84, #103, #111, #188, #189 and #216).

@thiagodefreitas: seeing as you offered in #53: could you see whether you remember why this code is the way it is?

[gavanderhoorn]

Note that what is discussed in #219 (in_motion not accurately reflecting robot state) also influences some of the mentioned issues, but that is only tangentially related.

[gavanderhoorn]

Ping @thiagodefreitas.

[gavanderhoorn]

After more inspection of the current JTA implementation and the history of the various related files and commits, it seems that the differences between both goal callbacks were an oversight when the multi-group support was introduced in ba3ca9a. Multi-group support for the JTA was first added in a separate file (joint_trajectory_action2.cpp), but even there the regular goal cb wasn't a copy of the one in industrial_robot_client (see this version which is from around the same time).

The goal callback associated with the global action server (ie: for the whole robot or one with a single motion group configured) is apparently functional, but the code is not very robust and there are multiple control flows possible that lead to (eventual) failure.

Suggestion: reuse (a significant portion of) the code from the per-group goal callback or the code in industrial_robot_client.

Unfortunately I won't have time to work on this now, so I'm leaving this comment for either my future self, or another future reader.

[Danfoa]

@gavanderhoorn I try to fix (and think I did) the problems mentioned in this issue on the second commit of the PR #259.

There were several errors (or at least they seemed like errors) on the handling of multi-group goals, and I try to solve them in a way consistent to the current development style. It would be very appreciated if you provide feedback on it.