MoveGroup API gets stuck in plan(...)

[Tobias Fromm]

moveit_users,

I recently tripped into an issue with the MoveGroup API which gets stuck
when planning a trajectory.

Specifically, when I run the code snippet pasted below, MoveGroup first
seems to plan the path like usual when I press the planning button in
the RViz plugin.
But then, moveit_ros gets stuck in the plan(...) function in
planning_interface/move_group_interface/src/move_group.cpp in line 471:
if (!move_action_client_->waitForResult()) and never returns from the
plan(...) function.

However, the planning seems to work fine as you can see in the output
below. Also, if I use setRandomTarget() instead of setJointValueTarget()
or setPoseTarget(), which I also tried, the planning functions returns
as expected.

I know that it doesn't make much sense to plan to the current position
as in this code snippet, but that's the simplest possible use case to
reproduce the issue.

Does anybody have an idea whether I forgot to set something or used the
plan function wrong? Any other ideas?

Cheers,
Tobi

-------------

Code snippet:

boost::shared_ptr<move_group_interface::MoveGroup> moveGroupPtr;
[...]
moveGroupPtr = boost::shared_ptr<move_group_interface::MoveGroup>(new
move_group_interface::MoveGroup("arm"));
moveGroupPtr->setPlannerId("arm[RRTStarkConfigDefault]");
moveGroupPtr->setPlanningTime(2.);
[...]
moveGroupPtr->setJointValueTarget(*moveGroupPtr->getCurrentState());
moveit::planning_interface::MoveGroup::Plan plan;
bool ret = moveGroupPtr->plan(plan);

MoveGroup output:

[ INFO] [1377092804.819722729]: Planning request received for MoveGroup
action. Forwarding to planning pipeline.
[ INFO] [1377092804.826220960]: Planner configuration
'arm[RRTStarkConfigDefault]' will use planner 'geometric::RRTstar'.
Additional configuration parameters will be set when the planner is
constructed.
[ INFO] [1377092804.827309191]: No optimization objective specified.
Defaulting to optimization of path length for the allowed planning time.
[ INFO] [1377092804.828668447]: Starting with 1 states
[ INFO] [1377092804.828839422]: Initial k-nearest value of 3
[ INFO] [1377092806.827681524]: Created 207 new states. Checked 2962
rewire options. 10 goal states in tree.
[ INFO] [1377092806.827743679]: Solution found in 2.000787 seconds

--
Tobias Fromm
Robotics Group
Jacobs University

Research I, Room 38 +49-421-200-3106
Campus Ring 1 t.f...@jacobs-university.de
28759 Bremen, Germany http://robotics.jacobs-university.de

[Ioan Sucan]

Hello Tobias,

Sorry for the delayed reply! Can you send me the rest of the code snippet? Do you have a ros::spin thread? (or async spinner)

It sounds like the action is not getting properly initialized maybe, or it is not getting data.

Another thing perhaps, before:  bool ret = moveGroupPtr->plan(plan); insert a sleep(5), just to make sure everything got initialized.

Let me know if you still see trouble.

One more thing: Is this code snippet executed in a ROS callback? Eg, service call, topic subscriber callback?

If so, that is the problem! You will need multiple callback processing queues for this to work properly, or better, move the code outside of a ROS callback.

Ioan

[Tobias Fromm]

Hi Ioan,

you nailed it, indeed the mistake was that I tried to run the plan
function inside a service call. Good to know that this doesn't work.

Thanks for your support!

Best,
Tobi

On 08/25/2013 12:36 PM, Ioan Sucan wrote:
> Hello Tobias,
>
> Sorry for the delayed reply! Can you send me the rest of the code snippet?
> Do you have a ros::spin thread? (or async spinner)
> It sounds like the action is not getting properly initialized maybe, or it
> is not getting data.
>
> Another thing perhaps, before: bool ret = moveGroupPtr->plan(plan); insert
> a sleep(5), just to make sure everything got initialized.
> Let me know if you still see trouble.
>
> One more thing: Is this code snippet executed in a ROS callback? Eg,
> service call, topic subscriber callback?
> If so, that is the problem! You will need multiple callback processing
> queues for this to work properly, or better, move the code outside of a ROS
> callback.
>
> Ioan
>
>
> On Wed, Aug 21, 2013 at 5:05 PM, Tobias Fromm
> <t.f...@jacobs-university.de>wrote:
>
>> moveit_users,
>>
>> I recently tripped into an issue with the MoveGroup API which gets stuck
>> when planning a trajectory.
>>
>> Specifically, when I run the code snippet pasted below, MoveGroup first
>> seems to plan the path like usual when I press the planning button in the
>> RViz plugin.
>> But then, moveit_ros gets stuck in the plan(...) function in

>> planning_interface/move_group_**interface/src/move_group.cpp in line 471:
>> if (!move_action_client_->**waitForResult()) and never returns from the

>> plan(...) function.
>>
>> However, the planning seems to work fine as you can see in the output
>> below. Also, if I use setRandomTarget() instead of setJointValueTarget() or
>> setPoseTarget(), which I also tried, the planning functions returns as
>> expected.
>>
>> I know that it doesn't make much sense to plan to the current position as
>> in this code snippet, but that's the simplest possible use case to
>> reproduce the issue.
>>
>> Does anybody have an idea whether I forgot to set something or used the
>> plan function wrong? Any other ideas?
>>
>> Cheers,
>> Tobi
>>
>> -------------
>>
>> Code snippet:
>>

>> boost::shared_ptr<move_group_**interface::MoveGroup> moveGroupPtr;
>> [...]
>> moveGroupPtr = boost::shared_ptr<move_group_**interface::MoveGroup>(new
>> move_group_interface::**MoveGroup("arm"));
>> moveGroupPtr->setPlannerId("**arm[RRTStarkConfigDefault]");
>> moveGroupPtr->setPlanningTime(**2.);
>> [...]
>> moveGroupPtr->**setJointValueTarget(***moveGroupPtr->getCurrentState(**
>> ));
>> moveit::planning_interface::**MoveGroup::Plan plan;

>> bool ret = moveGroupPtr->plan(plan);
>>
>> MoveGroup output:
>>
>> [ INFO] [1377092804.819722729]: Planning request received for MoveGroup
>> action. Forwarding to planning pipeline.
>> [ INFO] [1377092804.826220960]: Planner configuration
>> 'arm[RRTStarkConfigDefault]' will use planner 'geometric::RRTstar'.
>> Additional configuration parameters will be set when the planner is
>> constructed.
>> [ INFO] [1377092804.827309191]: No optimization objective specified.
>> Defaulting to optimization of path length for the allowed planning time.
>> [ INFO] [1377092804.828668447]: Starting with 1 states
>> [ INFO] [1377092804.828839422]: Initial k-nearest value of 3
>> [ INFO] [1377092806.827681524]: Created 207 new states. Checked 2962
>> rewire options. 10 goal states in tree.
>> [ INFO] [1377092806.827743679]: Solution found in 2.000787 seconds
>>
>> --
>> Tobias Fromm
>> Robotics Group
>> Jacobs University
>>
>> Research I, Room 38 +49-421-200-3106
>> Campus Ring 1 t.f...@jacobs-university.de

>> 28759 Bremen, Germany http://robotics.jacobs-**university.de<http://robotics.jacobs-university.de>

[Steffen P]

Hey,

I have exactly the same problem but with moveGroupPtr->pick(). The pick() function never returns, so my program stops working.

One more thing: Is this code snippet executed in a ROS callback? Eg, service call, topic subscriber callback?
If so, that is the problem! You will need multiple callback processing queues for this to work properly, or better, move the code outside of a ROS callback.

 

The pick() is called inside a callback. But how can I move the code out of the callback? Do I have to execute pick() in another thread?

Here is my code: https://github.com/fivef/kate_apps/blob/master/pick_and_place/src/pick_and_place.cpp

It works so far but only once :-)

I have now tried to put the pick() call into the spin loop:

ros::Rate r(10); // 1 hz

while (true)

{

if (app->clicked) {

app->clicked = false;

if (app->object_in_gripper) {

ROS_DEBUG("Object in gripper, place is called");

app->place();

} else {

app->pickup();

}

}

 ros::spinOnce();

 r.sleep();

}

very ugly and it still doesn't return...

Can someone help me please?

[Steffen P]

I have solved my problem by using a separate queue. Putting the pickup() call into the main loop didn't work.

What I have done:

#include <ros/callback_queue.h>

In Main: 

ros::CallbackQueue clicks_queue;

ros::SubscribeOptions options = ros::SubscribeOptions::create<geometry_msgs::PointStamped>("/clicked_point", 1, boost::bind(&Pick_and_place_app::receive_clicked_point_CB, app, _1)

, ros::VoidPtr(), &clicks_queue);

ros::AsyncSpinner spinner(0, &clicks_queue);

spinner.start();

ros::spin();

[Ioan Sucan]

Tobias, Steffen,

Sorry about the difficulties caused. I will work on using a separate callback queue internally, so you will not have to worry about this any more.

Ioan

[Ioan Sucan]

I looked at the possibility of adding a different callback queue in moveit, but the right way to do it is to make all NodeHandles use that callback queue. This makes things more complicated. In particular, when loading plugins, it is unclear how to pass that callback queue. Another option is to have a different callback queue for each plugin, but this makes things complicated.

So... I recommend you either use a separate callback queue yourself, or try using an AsyncSpinner with multiple threads.

Ioan