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.
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 ...
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.
(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)
I have added a concrete scenario.
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.
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.
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.
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.