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

Vijay Pradeep vpradeep at willowgarage.com
Thu Feb 3 21:33:44 UTC 2011


Hi Felix,

I'm still having trouble finding following lines in the tutorial:
*>>  while (ros::ok())
 >>    ros::spinOnce();
*
Can you give the full link to the page this is in?  This should probably be
changed to a *ros::spin()*

*Waiting for Server:*
Instead of calling *waitForServer()*, you can repeatedly call *
isServerConnected()*
Refer to the docs:
SimpleActionClient::isServerConnected()<http://www.ros.org/doc/api/actionlib/html/classactionlib_1_1SimpleActionClient.html#af3a32dd1b6e1486977c1dc99c98e627d>
The resulting code will look something like:

*  while (ros::ok() && !client.isServerConnected())
  {
    ros::Duration(0.1).sleep();
    ros::spinOnce();
  }
*
It sounds like you've resolved your other ActionClient issues, but
definitely let me know if you have any more questions.

Vijay

On Thu, Feb 3, 2011 at 7:57 AM, Felix Ingrand <felix at laas.fr> wrote:

> OK... found it...
>
> I should blame myself... for a beginner mistake (I guess some people are
> always beginner)
>
> The client I created was local to my function, hence was on the stack and
> would not survive the function call. So the whole thing was only working
> when the spin etc... were local to this scope... for some stupid reasons I
> though that C++ objects were created on the heap, not the stack... Using new
> and pointers immediately solved the problem.
>
> Sorry again for this, and thanks for you explanation. In fact, I plan to
> use the not_threaded version and properly handle the waitForServer (with
> proper timeout).
>
> The good news is that I can now call ROS actions from OpenPRS, and get
> proper feedback. yesss....
>
> Thanks again,
>
>
> On 3 févr. 2011, at 15:34, Felix Ingrand wrote:
>
> > In fact, I try removing (in my test code ) the ros::spin after the
> sendGoal, and I only get 2 callbacks (the active and the first feedback)
> then the client remains silent (but still alive). So I am not quite sure
> they run a ros::spin() in this thread.
> >
> > In any case, I am back to the original problem... which boils down now
> to:
> >
> > a ros::spin () or periodic call to ros::spinOnce() are required to get
> the client to properly run. if I put them in the function which call the
> sendGoal, it works (like with the fibonacci_callback_client exemple), if I
> make the  periodic calls to ros::spinOnce() later in another function, it
> does not work (in fact ros:ok() returns false then).
> >
> > I hope somebody has an idea as to why this is so.
> >
> > Thanks for your help,
> >
> > On 3 févr. 2011, at 15:00, Lorenz M=F6senlechner wrote:
> >
> >> Hi,
> >>
> >>>
> >>> On 3 févr. 2011, at 14:49, Lorenz M=F6senlechner wrote:
> >>>
> >>>> Hi,
> >>>>
> >>>> Where did you actually find that code? I cannot find it somehow.
> >>>
> >>>
> >>> http://www.ros.org/wiki/actionlib/Tutorials
> >>
> >> I was trying to find it there but couldn't find it in the first place
> >> :) Now I found it and I also don't quite understand why they use an
> >> explicit spinner instead of a sendGoalAndWait. Maybe one of the Willow
> >> guys can clarify here.
> >>
> >> Lorenz
> >>
> >>>
> >>>
> >>>>
> >>>> In the code below, a spin loop is required because the main function
> >>>> would terminate early otherwise. If you use ac.sendGoalAndWait, the
> >>>> explicit spin shouldn't be reqired.
> >>>>
> >>>> Btw. I guess your node eats up a lot of CPU because of the busy spin
> >>>> loop. As far as I remember, spinOnce returns immediately if there are
> >>>> no messages to handle. I guess it would be better to either use
> >>>> ros::spin() or add a ros rate with a sleep or so.
> >>>
> >>> Sure, but this was just for testing, in y "regular" code, I call
> >>> spinOnce at 100 Hz.
> >>>
> >>>
> >>>>
> >>>> Lorenz
> >>>>
> >>>>
> >>>>> Still, another thing which puzzle me, if the action client does the
> ros::spin(); why is there one following the sendGoal in the tutorial code?
> >>>>>
> >>>>> int main (int argc, char **argv)
> >>>>> {
> >>>>> ros::init(argc, argv, "test_fibonacci_callback");
> >>>>>
> >>>>> // 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
> >>>>> FibonacciGoal goal;
> >>>>> goal.order = 20;
> >>>>> ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
> >>>>>
> >>>>> while (ros::ok())
> >>>>>  ros::spinOnce();
> >>>>>
> >>>>> return 0;
> >>>>> }
> >>>>>
> >>>>> Moreover, why do not I see the other callbacks being called? and why
> do I see them being called if I add a spin just after the sendGoal... this
> is still not quite consistent.
> >>>>>
> >>>>> On 3 fÃ∞©vr. 2011, at 14:05, Felix Ingrand wrote:
> >>>>>
> >>>>>> Thanks,  now that makes sense.
> >>>>>>
> >>>>>> Still, this does not explain why later my ros::ok() returns false...
> or does that mean that only the thread which does the spin is allowed to do
> a ros::ok() and ros::spin()?
> >>>>>>
> >>>>>> In any case, I will try your method.
> >>>>>>
> >>>>>> As a side question, is it possible to make a waitForServer non
> blocking? (e.g. returning false until the server has started)
> >>>>>>
> >>>>>> Thanks again, that clarifies my understanding.
> >>>>>>
> >>>>>>
> >>>>>> On 3 fÃ∞©vr. 2011, at 13:42, Lorenz MÃ∞¶senlechner wrote:
> >>>>>>
> >>>>>>> Hi,
> >>>>>>>
> >>>>>>> in your example you set the spin_thread parameter of the action
> client
> >>>>>>> to true which causes it to internally start a thread that calls
> >>>>>>> ros::spin(). If you don't want that, you need to set the
> spin_thread
> >>>>>>> parameter to false. You also need to remove your waitForServer call
> >>>>>>> then since it would block forever.
> >>>>>>>
> >>>>>>> Lorenz
> >>>>>>>
> >>>>>>>>
> >>>>>>>> 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
> > Graphical design beauty http://tinyurl.com/28cjdlh
> >
> > _______________________________________________
> > ros-users mailing list
> > ros-users at code.ros.org
> > https://code.ros.org/mailman/listinfo/ros-users
> >
>
> --
>     Felix
> Il ne vous reste plus qu'une heure, quarante six minutes et quarante trois
> secondes de tranquillité d'esprit:
> http://tinyurl.com/5mlnfc
>
>
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>



-- 
Vijay Pradeep
Systems Engineer
Willow Garage, Inc.
 <tfoote at willowgarage.com>vpradeep at willowgarage.com
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20110203/a2e589a8/attachment-0003.html>


More information about the ros-users mailing list