As the problem is due to the infinite "while" loop that we can not
(well, can, according to the previous solution, but probably should not)
place in a fanction that is called by timer... let's remove the
"while" loop! Instead, we can use the final state machine that
can be called by timer.
charger_navigator.py:
To use this code:
import time # Time library from geometry_msgs.msg import PoseStamped from rclpy.duration import Duration import rclpy from basic_navigator import BasicNavigator, NavigationResult from charger_navigator import ChargerNavigator from rclpy.executors import SingleThreadedExecutor def main(args=None): try: rclpy.init(args=args) navigator = ChargerNavigator() executor = SingleThreadedExecutor() try: executor.spin() finally: executor.shutdown() navigator.destroy_node() finally: rclpy.shutdown() if __name__ == '__main__': main()
As you can see, we no longer need running anything in a separate thread. We also have more control over our code.
Here is the output of our program. As you can see, it runs over waypoints, and whenever battery level drops, switches from "patrolling" to charging.
An interesting twist. In theory, this should work:
rclpy.spin(navigator) # instead of executor.spin()
But it doesn't: subscription functions are not called.