Ask Your Question
1

Why is RobotState not the same in different nodes?

asked 2018-09-04 03:29:29 -0600

fvd gravatar image

updated 2018-09-04 04:04:43 -0600

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2018-09-04 06:13:26 -0600

v4hn gravatar image

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.

edit flag offensive delete link more

Comments

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.

fvd gravatar imagefvd ( 2018-09-04 08:32:56 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-09-04 03:29:29 -0600

Seen: 92 times

Last updated: Sep 04 '18