0
$\begingroup$

Rosanswers logo

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<const robot_state::AttachedBody*> 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.


Originally posted by fvd on ROS Answers with karma: 2180 on 2018-09-04

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi there,

I can't say I have a plain answer to your problem because it depends on your specific node setup.

Your second edit is right in that the PlanningSceneInterface.apply* functions only affect the move_group node. This is because the functions use the ros service provided by the move_group node. If you instantiate your own PlanningSceneMonitor in a different node, you have to connect it to move_group/monitored_planning_scene to receive the updates. But these are throttled (2 Hz by default I think) and of course updates there are asynchronous.

Historically the only way to update the scene was asynchronously via the add* functions (published messages). There all nodes/monitors got the updates.... or not because messages where dropped/not connected/...

The proper way to go for you might be to get a fresh planning scene from the move_group node right when you need it. You can use the GetPlanningScene service for that, or better, you utilize the PlanningSceneInterface and just add a method that exposes the full possibilities of the service there.


Originally posted by v4hn with karma: 2950 on 2018-09-04

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by fvd on 2018-09-04:
Thanks for the answer. I already made it a MoveGroupCapability. Is there a reason that move_group_interface does not include AttachedBody objects or even the whole scene? I see that it instantiates a CurrentStateMonitor, but that only tracks TF and JointStates.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.