c++ ROS-friendly state machine or similar
How do I coordinate a small sequence of ROS topic interactions in c++?
When receiving a certain ROS callback, I need to execute a small sequence like:
- Call ROS service S1
- Wait for some input X on topic T1
- Wait for some input Y on topic T2
- Call ROS service S2
Any of the steps may fail or timeout.
As far as I understand I should avoid waiting for a callback inside a callback. So should I spawn a thread to perform steps 1-4? And make the callbacks for topics T1 and T2 seeomehow coordinate how my thread progresses?
There must be some well-proven approaches?
Could be something similar to SMACH, but in c++ and more suited for lower-level high performance stuff.
Or it could be something with threads/locks/condition_variables/... ?
[EDIT] Here is a concrete scenario where I need this:
I have a ros node that controls some IO for a motor, offering a ros service for sending commands to the motor. The motor acknowledges all commands (as async io) and the driver node publishes these ack's on a topic along with data on what is being acknowledged.
I have another node that offers a ROS action "move". When receiving a "move" action goal, I forward the goal parameters as individual commands for the motor driver. For example:
- Receive move action goal callback
- Send goal.speed to hardware driver (ros service call)
- Listen for acknowledge of "setSpeed" cmd (published by hardware driver)
- Send goal.acceleration to hardware driver (ros service call)
- Listen for acknowledge of "setAcceleration" cmd (published by hardware driver)
- Send command "Run" to hardware driver (ros service call)
- Listen for acknowledge of "Run" cmd (published by hardware driver)
- ... the move is now considered active and the handling of the action goal may go on ...
In other scenarios the sequence might include requesting data from the hardware driver as one of the steps, data which also arrives asynchronously on a topic.
Asked by knxa on 2018-03-19 11:48:36 UTC
Comments
I suppose it should be possible to force your idea using threads and, if necessary, multiple callback queues (See Callbacks and Spinning in the wiki), but ...
Asked by Maarten on 2018-03-20 10:17:26 UTC
this is probably not the best method. If possible, it's probably better to listen to the certain ROS callback, only call S1 there, listen to T1 and T2, cache the inputs (including a timestamp) and call S2 if all conditions are met. Probably, we can give a good answer if you tell us the real problem.
Asked by Maarten on 2018-03-20 10:20:57 UTC
(that's why I put everything in comments and not as an answer: this is in my opinion the best answer for your question as it is written now, but not for your real problem)
Asked by Maarten on 2018-03-20 10:22:11 UTC
I have added a concrete scenario.
Asked by knxa on 2018-03-20 12:06:05 UTC
Why are setSpeed, setAcceleration and Run services if you have to wait for an acknowledge using a subscriber? Did you write the ROS driver yourself? Because in case of a service, I would implement the acknowledge in the return value.
Asked by Maarten on 2018-03-21 03:18:25 UTC
Anyway, a possible approach is to implement the state function in a separate function. Every input is stored and at the end of every callback, you call that (state) function. It checks what it has to do, given the current state and the new data received, does what needs to be done, and returns.
Asked by Maarten on 2018-03-21 03:21:40 UTC
Thank you, Maarten, for your input. It is not feasible to include the ack in the return value. It would highly complicate the driver. Also I want it asynchronous. For example to have the freedom to send both setSpeed and setAcceleration, before evaluating the of 'ack' replies.
Asked by knxa on 2018-03-21 04:08:12 UTC
Usually I prefer services if the calling node relies on one and only one receiver, and if it doesn't make sense to move on if that receiver did not receive the message. I find troubleshooting easier then, since services will check this for me.
Asked by knxa on 2018-03-21 04:08:47 UTC
I did use a topic at some point, and I sometimes consider reverting to it, to see how it affects performance. The driver is shared with other nodes, so it has to handle quite a number of service requests.
Asked by knxa on 2018-03-21 04:09:13 UTC
Makes sense. In that case, I would use the callbacks to update the state, and perform the necessary actions using either a seperate function that you call at the end of each callback or (if you need some timed events or so) replace the ros::spin() by a custom loop incorporating a ros::spinOnce().
Asked by Maarten on 2018-03-21 04:12:46 UTC