Multiple action clients to one server and correct preemption notifications
I have a JointTrajectoryController controlling some joints of a robot arm. Per default, a camera control node commands these joints to pan and tilt a camera. It uses the available action interface for that. The joints are however also part of a manipulator chain, so when the arm is commanded to move via MoveIt! I want this command from move_group to preempt the camera control node, notifying it that it got preempted and thus stopping it from sending any more Action goals. While this sounds easy enough in theory, it seems things don't quite work as I expected/hoped.
Given that when sending a goal, a action client always generates a goal ID (seems to be true for simple as well as "normal" clients), I thought that it should be possible to distinguish between state transitions by the node's client and those from other clients by looking at this ID (e.g. by remembering the latest one sent and comparing it those from all incoming state transitions). It seems this is not exposed anywhere in the action client code, however. What is available is a GoalHandle, so I thought that this can be used, internally also representing the goal ID. This does not seem to be the case, however.
When sending a goal from the action client in my node, I save the goal handle:
latest_gh_ = joint_traj_client_->sendGoal(goal, boost::bind(&CamJointTrajControl::transistionCb, this, _1));
In the transition callback, I compare the saved goal handle to the one provided by the callback. My hope was that transitions triggered by calls from other clients would fail the comparison to the saved one:
void CamJointTrajControl::transistionCb(actionlib::ClientGoalHandle<control_msgs::FollowJointTrajectoryAction> gh)
{
if (!(gh == latest_gh_)){
ROS_ERROR("Different goal handles!");
}
}
This however never outputs the ROS_ERROR message, no matter if I preempt the controllers using the rqt GUI (via the command interface) or if I perform planning using move_group (which uses the action interface, but from a different client).
I thus have two questions:
- When are two GoalHandle objects
supposed to be different from each
other/When is
operator==
useful for them? - What is the proper way of making
different action clients aware the
server has been preempted
correctly?
Experimental code is available in the debug_preemption_issue branch here.
/edit: It seems that this Q/A on SimpleActionClient callback for pre-empted goals is somewhat similar to what I want to achieve. Can't say I like the answers there though :D