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.