move_group.plan() didn't take attachedBody into account in moveit (indigo)

[Hao]

Hi, moveit users & developers,

I wrote a robot pick & place application with Hydro months ago, the code basically looks like this:

group.plan(plan);

group.execute(plan);

group.attachObject(object_name);

group.plan(plan);

group.execute(plan);

and it works fine, the motion plan takes the attached collision object into account, and I can see a green object moving together with my robot arm when visualize the plan in rviz.

However, after upgraded (my upgrade I mean re-install, you know that :P ) to Ubuntu 14.04, ROS indigo and moveit indigo recently, the same code won't take attached collision object into account while planning, and I can't see the green object in rviz when visualizing the plan.

So I modified my code like this:

group.plan(plan);

group.execute(plan);

group.attachObject(object_name);

group.plan(plan);

ROS_INTO_STREAM(plan.start_state_);

group.execute(plan);

And the attached_collision_objects[] vector is empty.

However, whenever the group.attachObject() is called, corresponding change happens in rviz : start_state(green) , goal_state(orange)  as well as scene robot (purple) will all show the attached object with corresponding color. Although we can observe clear collision between the purple object in scene robot and other collision object (yellow) in the scene when executing my code, the moveit_rviz_plugin clearly took the collision object into account and when visualizing a plan (after I clicked "Plan" button), the attached object will show up in green during the animation. 

Something else I noticed:

If I add  group.plan() between group.attachObject() and group.detachObject() in the move_group_interface tutorial code, there will be no collision object attached to PR2.

However, if I ran the pick_and_place_tutorial, after group.place(object, loc) returns, rviz will show a planned path with the box attached to the tip of right hand of PR2.

I am not sure if it's only me or someone else have met this problem too, so I ask here before report it as a bug.

Best,

Hao

--

我 Build ドラえもん

[Hao]

Had anyone confirmed this? Cause I'm pretty sure now it's a bug.

--

我 Build ドラえもん

[Simon Schmeißer]

Did you find a good solution? I have the same problem and think it's a bug as well

[Hao]

Hi, Simon,

I moved back to hydro due to a deadline. I might start tapping on this problem after July, but not now. I do encourage you to start working on that, and I would be happy to provide any insights I have and any previous effort I've put on this issue.

--

我 Build ドラえもん

[Simon Schmeißer]

Hi Hao,

please have a look at this post, it's an acceptable workaround for me at the moment:

https://groups.google.com/d/msg/moveit-users/ZYsWL1jRWyY/K0ia02WC4JYJ

make sure to scope the second part of the code with { } or put it in a function to kill the lock object before using it again.