Ways to transition between robot motion nodes
I'm trying to figure out how to transition between two motions for a humanoid robot at runtime. In my existing codebase, my whole motion code is in one thread with an "if" statement determining which motion generator to call. I want to isolate out the different types of motions into different nodes, for testability and so I can swap out a certain motion for another easily.
Let's say, I have three nodes.
- Node 1: determines whether a humanoid robot should be walking, or has fallen and should be performing a get up using the IMU.
- Node 2 (Walk Node): A node that is subscribed to joint positions, and publishes joint commands to walk.
- Node 3 (Getup Node): A node that is subscribed to joint positions, and publishes joint commands to get up.
Both the Walk and Getup Nodes listen to the joint positions and publish joint commands on the same channels. If they publish on the same joint command topic at the same time this will screw things up.
A case would be: the robot is walking, and suddenly a fall is detected in the IMU in Node 1, should Node 1 send a "stop" to Node 2 to stop the walking, and then publish a "start" to Node 3 to start the getup?
Another case would be: The getup has completed, should Node 3 publish a "done" to Node1, so then Node1 can notify Node2 to start?
Is it possible to do this sort of inter-node communication at a rate of ~100Hz? If there is a common way of doing this sort of "transitioning" between nodes, I'd be interested to hear more.
I'm using ROS2 galactic.
