Go to battery charger

Quick and (very) dirty solution

In the "Links" section, you can see some Internet gurus implementing one of the possible approaches. Let's take a closer look at what aurtors are doing, as well as at possible alternatives.

The code uses the BasicNavigator class (same as in Waypoint Follower example in one of the earlier sections). In addition to BasicNavigator class functionality, we need to be able to run an infinite cycle (navigate through the list of waypoints one time after another) and to switch to battery charging and back.

To do it, we can create another class that has the "patrolling" (wrapping waypoint follower into an infinite loop) and "charging" functionality, and has a BasicNavigator class as a member variable:

                    class ChargerNavigator(Node):
                        def __init__(self):
                            self.navigator = BasicNavigator()

Then we need to run the infinite loop, that handles all the navigation, and recreates route when goal is reached. In the "waypoint follower" example earlier, we have handled it by simply placing the "while" cycle in the main function.

                    def main():
                        navigator = BasicNavigator()
                        # set our demo's goal poses to follow
                        goal_poses = []
                        goal_pose1 = PoseStamped()
                        goal_pose1.header.frame_id = 'map'
                        goal_pose1.header.stamp = navigator.get_clock().now().to_msg()
                        goal_pose1.pose.position.x = 1.0
                        goal_pose1.pose.position.y = 2.15
                        goal_pose1.pose.orientation.w = 1.0
                        goal_pose1.pose.orientation.z = 0.0
                        # additional goals can be appended
                        goal_pose2 = PoseStamped()
                        goal_pose2.header.frame_id = 'map'
                        goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
                        goal_pose2.pose.position.x = 1.5
                        goal_pose2.pose.position.y = -3.75
                        goal_pose2.pose.orientation.w = 0.707
                        goal_pose2.pose.orientation.z = 0.707

                        while not navigator.isNavComplete():

There was nothing wrong with this approach earlier, but now we need to perform an extra step: we need to subscribe to a ROS2 battery level message and change code's behavior when battery is discharged. Here is the problem.

See, an infinite loop running in the main thread is a blocking thing and a second function, one that handles subscription to battery info, will never be called.

To overcome this obstacle, people usually use the MultiThreadedExecutor. First, we need to create a second class (that is also node-based):

                    import time  # Time library
                    from rclpy.duration import Duration
                    import rclpy                             
                    from rclpy.node import Node
                    from rclpy.executors import MultiThreadedExecutor
                    from basic_navigator import BasicNavigator, NavigationResult
                    from geometry_msgs.msg import PoseStamped
                    from geometry_msgs.msg import Twist      
                    from sensor_msgs.msg import BatteryState 
                    import time
                    class BatteryStatusPublisher(Node):
                        def __init__(self):
                            self.subscription_battery_status_not_used_except_for_scope = self.create_subscription(
                        def get_battery_status(self, msg: BatteryState):
                            self.nBatteryLevel = msg.percentage
                            print("Battery at", self.nBatteryLevel, "%")


Let me also provide the complete ChargerNavigator code (though I never tested it properly, so do not be surprised if you get a negative battery level). This code is mostly copied from "Links" below, it should work, but I do not recommend this approach. Later I will explain why.


To call this code, we can use the following "main" function:

                    import time # Time library
                    from geometry_msgs.msg import PoseStamped # Pose with ref frame and timestamp
                    from rclpy.duration import Duration # Handles time for ROS 2
                    import rclpy # Python client library for ROS 2
                    from basic_navigator import BasicNavigator, NavigationResult # Helper module
                    from charger_navigator import ChargerNavigator #, BatteryStatusPublisher
                    #from rclpy.executors import MultiThreadedExecutor
                    from rclpy.executors import SingleThreadedExecutor
                    def main(args=None):
                            navigator = ChargerNavigator()
                            batteryStatusPublisher = BatteryStatusPublisher()
                            executor = MultiThreadedExecutor(num_threads=4)
                    if __name__ == '__main__':

Let me repeat: this code works. But... what is it doing? We (well, not we - they!) create a ChargerNavigator class, that has a timer in its init() function. Then we use a blocking call, and to avoid consequences, we start another node in another thread! Ugly. And not just ugly: if something goes wrong, it is going to be very difficult to control resources leaking.

(C) snowcron.com, all rights reserved

Please read the disclaimer