diff --git a/turtlebot4_python_tutorials/setup.py b/turtlebot4_python_tutorials/setup.py index 4a63e98..2cb4382 100644 --- a/turtlebot4_python_tutorials/setup.py +++ b/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', ], }, ) diff --git a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/mail_delivery.py b/turtlebot4_python_tutorials/turtlebot4_python_tutorials/mail_delivery.py new file mode 100644 index 0000000..a8482bc --- /dev/null +++ b/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() diff --git a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_through_poses.py b/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_through_poses.py index 0f04fd5..b1a8f46 100644 --- a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_through_poses.py +++ b/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() diff --git a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py b/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py index fce9235..093063b 100644 --- a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py +++ b/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() diff --git a/turtlebot4_python_tutorials/turtlebot4_python_tutorials/patrol_loop.py b/turtlebot4_python_tutorials/turtlebot4_python_tutorials/patrol_loop.py new file mode 100644 index 0000000..5df7e63 --- /dev/null +++ b/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()