[ros-users] Weird ros::ok() actionlib::SimpleActionClient interactions...

Felix Ingrand felix at laas.fr
Thu Feb 3 11:36:19 UTC 2011


On 3 févr. 2011, at 11:36, Lorenz Mösenlechner wrote:

> Hi,
> 
> the action client starts up its own spin thread per default. You can
> disable it by instantiating it as follows:
> 
> Client ac("fibonacci", false);
> 
> But you need to be a little bit careful with that. If you don't have a
> separate spin thread running, waitForServer will not work. It will
> just block because it needs a spinner running in order to get message
> callbacks for the action's status topic.

I cannot do this, as my program is not multithreaded (at least for now).

> 
> If I understand your example correctly, you are trying to start up an
> action client, send a goal and sort of block until you receive a
> result. After you got it, you try to shut down the whole ros loop
> again.

I did that just for testing callbacks are properly working. But what I want to do is start an action client, and then have my spinOnce (which is executed in the same main thread but later and in other part of the code) properly calling the callback. But for some reasons if the ros::ok()/ros::spinOnce() code is just after the sendGoal, it works, but if it is in other part of the code, it fails...

> Can't you just let the spinner run and use ac.sendGoalAndWait
> instead to do a blocking call?
> 
> Lorenz
> 
>> Hi,
>> 
>> I am getting OpenPRS connected to ROS to supervise our PR2.
>> 
>> So far, I have done some testing with topics (subscribing and publishing)/services and all is good.
>> 
>> Trying to play with SimpleAction (which seem to be the best tool in ROS to supervise nodes) I am witnessing a weird problem.
>> 
>> - the version of OpenPRS I use is NOT multithreaded, but it uses SIGALARM
>> - I have an evaluable predicate which automatically and periodically (100 Hz) call 
>>  if (ros::ok()) {
>>    ros::spinOnce();
>>  }
>> and this takes care of all the callbacks (and indeed, it works with subscribed topics).
>> 
>> Now, when I create a SimpleAction client (using the Fibonacci server example) with this code:
>> 
>> if (PUGetOprsParameters(terms, 1,
>> 			  INTEGER, &seq)) {
>>    // Create the action client
>>    Client ac("fibonacci", true);
>> 
>>    ROS_INFO("Waiting for action server to start.");
>>    ac.waitForServer();
>>    ROS_INFO("Action server started, sending goal.");
>> 
>>    // Send Goal
>>    learning_actionlib::FibonacciGoal goal;
>>    goal.order = seq;
>>    ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
>> 
>> It works, and shortly after (even before calling spinOnce explicitly), I get the activeCb called (once) and the feedbackCB called (only once with seq value 3). So I guess that there is an implicit spinOnce done somewhere here in the sendGoal.
>> 
>> If I put the following code just after the sendGoal:
>> 
>>    while (ros::ok()) {
>>    ros::spinOnce();		// This will call the callbacks.
>>    }
>> 
>> Then the sequence callback is called for all the values and the doneCB (to which I added at the end a ros::Shutdown to let the ros::ok return false and leave the loop) is also called. good...
>> 
>> But if I comment this loop, and rely on the evaluable predicate which will be called later by the OpenPRS main loop, then ros::ok() return true until I call the function doing the sendGoal, after which it returns false.
>> 
>> I am running all this on Ubuntu 10.4 (based on the VirtualBox machine proposed by WG, and properly up to date).
>> 
>> Before I dig further, any idea what I may be doing wrong here? I can see that SimpleActionClient starts a lot of threads, still the code I use to spin ROS after the sendGoal OR in the evaluable predicate are executing in the same thread (the main one)... why does it work when it is just "after" the sendGoal and not a few milliseconds later? 
>> 
>> Moreover, the SIGALARM handler does NOT execute the spinOnce, it merely flip a flag to tell the OpenPRS main loop to check for evaluable predicates.
>> 
>> Thanks for any help,
>> 
>> PS: is there a way to trace/see what is the reason why a node is suddenly returning ros::ok() false?
> 
> -- 
> Lorenz Mösenlechner            | moesenle at in.tum.de
> Technische Universität München | Boltzmannstr. 3
> 85748 Garching bei München     | Germany
> http://ias.cs.tum.edu/         | Tel: +49 (89) 289-26910
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
> 

-- 
     Felix
You think you are making economical rational decisions...
http://tinyurl.com/kvu9ck





More information about the ros-users mailing list