Skip to content

Commit 338aee6

Browse files
roni-kreininroni-kreinin
authored
Merge pull request #3 from turtlebot/hilary-luo/warehouse-world
Update for warehouse world
2 parents 5db92b3 + a05a7f0 commit 338aee6

File tree

5 files changed

+259
-7
lines changed

5 files changed

+259
-7
lines changed

‎turtlebot4_python_tutorials/setup.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@
2626
'nav_through_poses = turtlebot4_python_tutorials.nav_through_poses:main',
2727
'follow_waypoints = turtlebot4_python_tutorials.follow_waypoints:main',
2828
'create_path = turtlebot4_python_tutorials.create_path:main',
29+
'patrol_loop = turtlebot4_python_tutorials.patrol_loop:main',
30+
'mail_delivery = turtlebot4_python_tutorials.mail_delivery:main',
2931
],
3032
},
3133
)

‎turtlebot4_python_tutorials/turtlebot4_python_tutorials/mail_delivery.py‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2023 Clearpath Robotics, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
#
17+
# @author Hilary Luo (hluo@clearpathrobotics.com)
18+
19+
import rclpy
20+
21+
from turtlebot4_navigation.turtlebot4_navigator import TurtleBot4Directions, TurtleBot4Navigator
22+
23+
24+
def main(args=None):
25+
rclpy.init(args=args)
26+
27+
navigator = TurtleBot4Navigator()
28+
29+
# Start on dock
30+
if not navigator.getDockedStatus():
31+
navigator.info('Docking before intialising pose')
32+
navigator.dock()
33+
34+
# Set initial pose
35+
initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
36+
navigator.setInitialPose(initial_pose)
37+
38+
# Wait for Nav2
39+
navigator.waitUntilNav2Active()
40+
41+
# Undock
42+
navigator.undock()
43+
44+
# Prepare goal pose options
45+
goal_options = [
46+
{'name': 'Home',
47+
'pose': navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST)},
48+
49+
{'name': 'Position 1',
50+
'pose': navigator.getPoseStamped([10.0, 6.0], TurtleBot4Directions.EAST)},
51+
52+
{'name': 'Position 2',
53+
'pose': navigator.getPoseStamped([-9.0, 9.0], TurtleBot4Directions.NORTH)},
54+
55+
{'name': 'Position 3',
56+
'pose': navigator.getPoseStamped([-12.0, 2.0], TurtleBot4Directions.NORTH_WEST)},
57+
58+
{'name': 'Position 4',
59+
'pose': navigator.getPoseStamped([3.0, -7.0], TurtleBot4Directions.WEST)},
60+
61+
{'name': 'Exit',
62+
'pose': None}
63+
]
64+
65+
navigator.info('Welcome to the mail delivery service.')
66+
67+
while True:
68+
# Create a list of the goals for display
69+
options_str = 'Please enter the number corresponding to the desired robot goal position:\n'
70+
for i in range(len(goal_options)):
71+
options_str += f' {i}. {goal_options[i]["name"]}\n'
72+
73+
# Prompt the user for the goal location
74+
raw_input = input(f'{options_str}Selection: ')
75+
76+
selected_index = 0
77+
78+
# Verify that the value input is a number
79+
try:
80+
selected_index = int(raw_input)
81+
except ValueError:
82+
navigator.error(f'Invalid goal selection: {raw_input}')
83+
continue
84+
85+
# Verify that the user input is within a valid range
86+
if (selected_index < 0) or (selected_index >= len(goal_options)):
87+
navigator.error(f'Goal selection out of bounds: {selected_index}')
88+
89+
# Check for exit
90+
elif goal_options[selected_index]['name'] == 'Exit':
91+
break
92+
93+
else:
94+
# Navigate to requested position
95+
navigator.startToPose(goal_options[selected_index]['pose'])
96+
97+
rclpy.shutdown()
98+
99+
100+
if __name__ == '__main__':
101+
main()

‎turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_through_poses.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -40,12 +40,12 @@ def main():
4040

4141
# Set goal poses
4242
goal_pose = []
43-
goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
44-
goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
45-
goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
46-
goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
47-
goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
48-
goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))
43+
goal_pose.append(navigator.getPoseStamped([-3.0, 0.0], TurtleBot4Directions.EAST))
44+
goal_pose.append(navigator.getPoseStamped([-3.0, -3.0], TurtleBot4Directions.NORTH))
45+
goal_pose.append(navigator.getPoseStamped([3.0, -3.0], TurtleBot4Directions.NORTH_WEST))
46+
goal_pose.append(navigator.getPoseStamped([9.0, -1.0], TurtleBot4Directions.WEST))
47+
goal_pose.append(navigator.getPoseStamped([9.0, 1.0], TurtleBot4Directions.SOUTH))
48+
goal_pose.append(navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST))
4949

5050
# Undock
5151
navigator.undock()

‎turtlebot4_python_tutorials/turtlebot4_python_tutorials/nav_to_pose.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ def main():
3939
navigator.waitUntilNav2Active()
4040

4141
# Set goal poses
42-
goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)
42+
goal_pose = navigator.getPoseStamped([-13.0, 9.0], TurtleBot4Directions.EAST)
4343

4444
# Undock
4545
navigator.undock()

‎turtlebot4_python_tutorials/turtlebot4_python_tutorials/patrol_loop.py‎

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
#!/usr/bin/env python3
2+
3+
# Copyright 2023 Clearpath Robotics, Inc.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
#
17+
# @author Hilary Luo (hluo@clearpathrobotics.com)
18+
19+
from math import floor
20+
from threading import Lock, Thread
21+
from time import sleep
22+
23+
import rclpy
24+
25+
from rclpy.executors import SingleThreadedExecutor
26+
from rclpy.node import Node
27+
from rclpy.qos import qos_profile_sensor_data
28+
29+
from sensor_msgs.msg import BatteryState
30+
from turtlebot4_navigation.turtlebot4_navigator import TurtleBot4Directions, TurtleBot4Navigator
31+
32+
BATTERY_HIGH = 0.95
33+
BATTERY_LOW = 0.2 # when the robot will go charge
34+
BATTERY_CRITICAL = 0.1 # when the robot will shutdown
35+
36+
37+
class BatteryMonitor(Node):
38+
39+
def __init__(self, lock):
40+
super().__init__('battery_monitor')
41+
42+
self.lock = lock
43+
44+
# Subscribe to the /battery_state topic
45+
self.battery_state_subscriber = self.create_subscription(
46+
BatteryState,
47+
'battery_state',
48+
self.battery_state_callback,
49+
qos_profile_sensor_data)
50+
51+
# Callbacks
52+
def battery_state_callback(self, batt_msg: BatteryState):
53+
with self.lock:
54+
self.battery_percent = batt_msg.percentage
55+
56+
def thread_function(self):
57+
executor = SingleThreadedExecutor()
58+
executor.add_node(self)
59+
executor.spin()
60+
61+
62+
def main(args=None):
63+
rclpy.init(args=args)
64+
65+
lock = Lock()
66+
battery_monitor = BatteryMonitor(lock)
67+
68+
navigator = TurtleBot4Navigator()
69+
battery_percent = None
70+
position_index = 0
71+
72+
thread = Thread(target=battery_monitor.thread_function, daemon=True)
73+
thread.start()
74+
75+
# Start on dock
76+
if not navigator.getDockedStatus():
77+
navigator.info('Docking before intialising pose')
78+
navigator.dock()
79+
80+
# Set initial pose
81+
initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
82+
navigator.setInitialPose(initial_pose)
83+
84+
# Wait for Nav2
85+
navigator.waitUntilNav2Active()
86+
87+
# Undock
88+
navigator.undock()
89+
90+
# Prepare goal poses
91+
goal_pose = []
92+
goal_pose.append(navigator.getPoseStamped([-5.0, 1.0], TurtleBot4Directions.EAST))
93+
goal_pose.append(navigator.getPoseStamped([-5.0, -23.0], TurtleBot4Directions.NORTH))
94+
goal_pose.append(navigator.getPoseStamped([9.0, -23.0], TurtleBot4Directions.NORTH_WEST))
95+
goal_pose.append(navigator.getPoseStamped([10.0, 2.0], TurtleBot4Directions.WEST))
96+
97+
while True:
98+
with lock:
99+
battery_percent = battery_monitor.battery_percent
100+
101+
if (battery_percent is not None):
102+
navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
103+
104+
# Check battery charge level
105+
if (battery_percent < BATTERY_CRITICAL):
106+
navigator.error('Battery critically low. Charge or power down')
107+
break
108+
elif (battery_percent < BATTERY_LOW):
109+
# Go near the dock
110+
navigator.info('Docking for charge')
111+
navigator.startToPose(navigator.getPoseStamped([-1.0, 1.0],
112+
TurtleBot4Directions.EAST))
113+
navigator.dock()
114+
115+
if not navigator.getDockedStatus():
116+
navigator.error('Robot failed to dock')
117+
break
118+
119+
# Wait until charged
120+
navigator.info('Charging...')
121+
battery_percent_prev = 0
122+
while (battery_percent < BATTERY_HIGH):
123+
sleep(15)
124+
battery_percent_prev = floor(battery_percent*100)/100
125+
with lock:
126+
battery_percent = battery_monitor.battery_percent
127+
128+
# Print charge level every time it increases a percent
129+
if battery_percent > (battery_percent_prev + 0.01):
130+
navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
131+
132+
# Undock
133+
navigator.undock()
134+
position_index = 0
135+
136+
else:
137+
# Navigate to next position
138+
navigator.startToPose(goal_pose[position_index])
139+
140+
position_index = position_index + 1
141+
if position_index >= len(goal_pose):
142+
position_index = 0
143+
144+
battery_monitor.destroy_node()
145+
rclpy.shutdown()
146+
147+
148+
if __name__ == '__main__':
149+
main()

0 commit comments

Comments
 (0)