I am attaching a collision object to my robot. The object is displayed fine and reported correctly as the cause of a collision if I move it into a wall in Rviz. I also added some debug messages that make it clear that the RobotState in the move_group node knows about this object.
However, I have two other nodes running, which have a move_group_interface and a MoveGroupCommander in C++/Python respectively, and neither of their RobotStates seem to be aware of the attached object. They are both running when the AttachedCollisionObject is published, but their state does not change.
I check with this code in the C++ node:
robot_state::RobotState state(*group_interface.getCurrentState());
std::vector ab;
state.getAttachedBodies(ab);
ROS_INFO_STREAM("Current state has " << ab.size() << " attached bodies.");
But the result stays 0, even though it should say 1 after attaching the object (a similar debug message does report the correct number in the move_group node).
Normally it would not be a problem since the collisions are checked in the move_group node, but I need these attached objects to construct my planning goal. What am I missing? How can I update the RobotState? Thanks in advance.
I build MoveIt from source on the kinetic-devel branch and use ROS Kinetic on Ubuntu 16.04.
edit: I am using the PlanningSceneInterface to attach the collision object and not publishing a message manually, if that makes a difference.
edit2: I may have found the reason for this behavior in the description of the PlanningSceneInterface applyAttachedCollisionObject function:
Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene
I'm not sure how to do that or why that wouldn't be the default behavior of the move_group_interface, but any pointers are welcome.
↧