Using SMACH to monitor robot battery level
I'm wondering if anyone knows how to program the following simple scenario using SMACH.
The robot monitors its battery level on a topic called /battery_level using a SMACH MonitorState which is part of the state machine SM_MONITOR_BATTERY. In the meantime, the robot navigates around a series of waypoints in succession using a state machine SM_NAV_WAYPOINTS.
I want the overall state machine to behave as follows: If the battery level falls below a threshold (SM_MONITOR_BATTERY returns 'invalid'), SM_MONITOR_BATTERY transitions to another state called RECHARGE that moves the robot to the docking station (NAV_DOCKING_STATION) and recharges the robot. In the meantime, the SM_NAV_WAYPOINT machine is paused or terminated until the battery is recharged (SM_MONITOR_BATTERY returns 'valid').
I can make both the recharging behavior and the waypoint navigation behavior work on their own but I have only a vague idea how they can be combined using the Concurrence container. I've tried for days to get the right syntax and I have failed.
Any help would be greatly appreciated.
--patrick