Update for warehouse world

turtlebot4_python_tutorials/setup.py

@@ -26,6 +26,8 @@
             'nav_through_poses = turtlebot4_python_tutorials.nav_through_poses:main',
             'follow_waypoints = turtlebot4_python_tutorials.follow_waypoints:main',
             'create_path = turtlebot4_python_tutorials.create_path:main',
+            'patrol_loop = turtlebot4_python_tutorials.patrol_loop:main',
+            'mail_delivery = turtlebot4_python_tutorials.mail_delivery:main',
         ],
     },
 )

turtlebot4_python_tutorials/turtlebot4_python_tutorials/mail_delivery.py

@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 Clearpath Robotics, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# @author Hilary Luo (hluo@clearpathrobotics.com)
+
+import rclpy
+
+from turtlebot4_navigation.turtlebot4_navigator import TurtleBot4Directions, TurtleBot4Navigator
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    navigator = TurtleBot4Navigator()
+
+    # Start on dock
+    if not navigator.getDockedStatus():
+        navigator.info('Docking before intialising pose')
+        navigator.dock()
+
+    # Set initial pose
+    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
+    navigator.setInitialPose(initial_pose)
+
+    # Wait for Nav2
+    navigator.waitUntilNav2Active()
+
+    # Undock
+    navigator.undock()
+
+    # Prepare goal pose options
+    goal_options = [
+        {'name': 'Home',
+         'pose': navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST)},
+
+        {'name': 'Position 1',
+         'pose': navigator.getPoseStamped([10.0, 6.0], TurtleBot4Directions.EAST)},
+
+        {'name': 'Position 2',
+         'pose': navigator.getPoseStamped([-9.0, 9.0], TurtleBot4Directions.NORTH)},
+
+        {'name': 'Position 3',
+         'pose': navigator.getPoseStamped([-12.0, 2.0], TurtleBot4Directions.NORTH_WEST)},
+
+        {'name': 'Position 4',
+         'pose': navigator.getPoseStamped([3.0, -7.0], TurtleBot4Directions.WEST)},
+
+        {'name': 'Exit',
+         'pose': None}
+    ]
+
+    navigator.info('Welcome to the mail delivery service.')
+
+    while True:
+        # Create a list of the goals for display
+        options_str = 'Please enter the number corresponding to the desired robot goal position:\n'
+        for i in range(len(goal_options)):
+            options_str += f'    {i}. {goal_options[i]["name"]}\n'
+
+        # Prompt the user for the goal location
+        raw_input = input(f'{options_str}Selection: ')
+
+        selected_index = 0
+
+        # Verify that the value input is a number
+        try:
+            selected_index = int(raw_input)
+        except ValueError:
+            navigator.error(f'Invalid goal selection: {raw_input}')
+            continue
+
+        # Verify that the user input is within a valid range
+        if (selected_index < 0) or (selected_index >= len(goal_options)):
+            navigator.error(f'Goal selection out of bounds: {selected_index}')
+
+        # Check for exit
+        elif goal_options[selected_index]['name'] == 'Exit':
+            break
+
+        else:
+            # Navigate to requested position
+            navigator.startToPose(goal_options[selected_index]['pose'])
+
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_through_poses.py

@@ -40,12 +40,12 @@ def main():
 
     # Set goal poses
     goal_pose = []
-    goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
-    goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
-    goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
-    goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
-    goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
-    goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))
+    goal_pose.append(navigator.getPoseStamped([-3.0, 0.0], TurtleBot4Directions.EAST))
+    goal_pose.append(navigator.getPoseStamped([-3.0, -3.0], TurtleBot4Directions.NORTH))
+    goal_pose.append(navigator.getPoseStamped([3.0, -3.0], TurtleBot4Directions.NORTH_WEST))
+    goal_pose.append(navigator.getPoseStamped([9.0, -1.0], TurtleBot4Directions.WEST))
+    goal_pose.append(navigator.getPoseStamped([9.0, 1.0], TurtleBot4Directions.SOUTH))
+    goal_pose.append(navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST))
 
     # Undock
     navigator.undock()

turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py

@@ -39,7 +39,7 @@ def main():
     navigator.waitUntilNav2Active()
 
     # Set goal poses
-    goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)
+    goal_pose = navigator.getPoseStamped([-13.0, 9.0], TurtleBot4Directions.EAST)
 
     # Undock
     navigator.undock()

turtlebot4_python_tutorials/turtlebot4_python_tutorials/patrol_loop.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 Clearpath Robotics, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# @author Hilary Luo (hluo@clearpathrobotics.com)
+
+from math import floor
+from threading import Lock, Thread
+from time import sleep
+
+import rclpy
+
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+
+from sensor_msgs.msg import BatteryState
+from turtlebot4_navigation.turtlebot4_navigator import TurtleBot4Directions, TurtleBot4Navigator
+
+BATTERY_HIGH = 0.95
+BATTERY_LOW = 0.2  # when the robot will go charge
+BATTERY_CRITICAL = 0.1  # when the robot will shutdown
+
+
+class BatteryMonitor(Node):
+
+    def __init__(self, lock):
+        super().__init__('battery_monitor')
+
+        self.lock = lock
+
+        # Subscribe to the /battery_state topic
+        self.battery_state_subscriber = self.create_subscription(
+            BatteryState,
+            'battery_state',
+            self.battery_state_callback,
+            qos_profile_sensor_data)
+
+    # Callbacks
+    def battery_state_callback(self, batt_msg: BatteryState):
+        with self.lock:
+            self.battery_percent = batt_msg.percentage
+
+    def thread_function(self):
+        executor = SingleThreadedExecutor()
+        executor.add_node(self)
+        executor.spin()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    lock = Lock()
+    battery_monitor = BatteryMonitor(lock)
+
+    navigator = TurtleBot4Navigator()
+    battery_percent = None
+    position_index = 0
+
+    thread = Thread(target=battery_monitor.thread_function, daemon=True)
+    thread.start()
+
+    # Start on dock
+    if not navigator.getDockedStatus():
+        navigator.info('Docking before intialising pose')
+        navigator.dock()
+
+    # Set initial pose
+    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
+    navigator.setInitialPose(initial_pose)
+
+    # Wait for Nav2
+    navigator.waitUntilNav2Active()
+
+    # Undock
+    navigator.undock()
+
+    # Prepare goal poses
+    goal_pose = []
+    goal_pose.append(navigator.getPoseStamped([-5.0, 1.0], TurtleBot4Directions.EAST))
+    goal_pose.append(navigator.getPoseStamped([-5.0, -23.0], TurtleBot4Directions.NORTH))
+    goal_pose.append(navigator.getPoseStamped([9.0, -23.0], TurtleBot4Directions.NORTH_WEST))
+    goal_pose.append(navigator.getPoseStamped([10.0, 2.0], TurtleBot4Directions.WEST))
+
+    while True:
+        with lock:
+            battery_percent = battery_monitor.battery_percent
+
+        if (battery_percent is not None):
+            navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
+
+            # Check battery charge level
+            if (battery_percent < BATTERY_CRITICAL):
+                navigator.error('Battery critically low. Charge or power down')
+                break
+            elif (battery_percent < BATTERY_LOW):
+                # Go near the dock
+                navigator.info('Docking for charge')
+                navigator.startToPose(navigator.getPoseStamped([-1.0, 1.0],
+                                      TurtleBot4Directions.EAST))
+                navigator.dock()
+
+                if not navigator.getDockedStatus():
+                    navigator.error('Robot failed to dock')
+                    break
+
+                # Wait until charged
+                navigator.info('Charging...')
+                battery_percent_prev = 0
+                while (battery_percent < BATTERY_HIGH):
+                    sleep(15)
+                    battery_percent_prev = floor(battery_percent*100)/100
+                    with lock:
+                        battery_percent = battery_monitor.battery_percent
+
+                    # Print charge level every time it increases a percent
+                    if battery_percent > (battery_percent_prev + 0.01):
+                        navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
+
+                # Undock
+                navigator.undock()
+                position_index = 0
+
+            else:
+                # Navigate to next position
+                navigator.startToPose(goal_pose[position_index])
+
+                position_index = position_index + 1
+                if position_index >= len(goal_pose):
+                    position_index = 0
+
+    battery_monitor.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()