m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_simple_commander/nav2_simple_commander/__init__.py
View File
navigation2/nav2_simple_commander/nav2_simple_commander/costmap_2d.py
+192
View File
@@ -0,0 +1,192 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
# Copyright 2022 Stevedan Ogochukwu Omodolor
# Copyright 2022 Jaehun Jackson Kim
#
# 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.
"""
This is a Python3 API for costmap 2d messages from the stack.
It provides the basic conversion, get/set,
and handling semantics found in the costmap 2d C++ API.
"""
import numpy as np
class PyCostmap2D:
"""
PyCostmap2D.
Costmap Python3 API for OccupancyGrids to populate from published messages
"""
def __init__(self, occupancy_map):
"""
Initialize costmap2D.
Args:
----
occupancy_map (OccupancyGrid): 2D OccupancyGrid Map
"""
self.size_x = occupancy_map.info.width
self.size_y = occupancy_map.info.height
self.resolution = occupancy_map.info.resolution
self.origin_x = occupancy_map.info.origin.position.x
self.origin_y = occupancy_map.info.origin.position.y
self.global_frame_id = occupancy_map.header.frame_id
self.costmap_timestamp = occupancy_map.header.stamp
# Extract costmap
self.costmap = np.array(occupancy_map.data, dtype=np.uint8)
def getSizeInCellsX(self):
"""Get map width in cells."""
return self.size_x
def getSizeInCellsY(self):
"""Get map height in cells."""
return self.size_y
def getSizeInMetersX(self):
"""Get x axis map size in meters."""
return (self.size_x - 1 + 0.5) * self.resolution
def getSizeInMetersY(self):
"""Get y axis map size in meters."""
return (self.size_y - 1 + 0.5) * self.resolution
def getOriginX(self):
"""Get the origin x axis of the map [m]."""
return self.origin_x
def getOriginY(self):
"""Get the origin y axis of the map [m]."""
return self.origin_y
def getResolution(self):
"""Get map resolution [m/cell]."""
return self.resolution
def getGlobalFrameID(self):
"""Get global frame_id."""
return self.global_frame_id
def getCostmapTimestamp(self):
"""Get costmap timestamp."""
return self.costmap_timestamp
def getCostXY(self, mx: int, my: int) -> np.uint8:
"""
Get the cost of a cell in the costmap using map coordinate XY.
Args
----
mx (int): map coordinate X to get cost
my (int): map coordinate Y to get cost
Returns
-------
np.uint8: cost of a cell
"""
return self.costmap[self.getIndex(mx, my)]
def getCostIdx(self, index: int) -> np.uint8:
"""
Get the cost of a cell in the costmap using Index.
Args
----
index (int): index of cell to get cost
Returns
-------
np.uint8: cost of a cell
"""
return self.costmap[index]
def setCost(self, mx: int, my: int, cost: np.uint8) -> None:
"""
Set the cost of a cell in the costmap using map coordinate XY.
Args
----
mx (int): map coordinate X to get cost
my (int): map coordinate Y to get cost
cost (np.uint8): The cost to set the cell
Returns
-------
None
"""
self.costmap[self.getIndex(mx, my)] = cost
def mapToWorld(self, mx: int, my: int) -> tuple[float, float]:
"""
Get the world coordinate XY using map coordinate XY.
Args
----
mx (int): map coordinate X to get world coordinate
my (int): map coordinate Y to get world coordinate
Returns
-------
tuple of float: wx, wy
wx (float) [m]: world coordinate X
wy (float) [m]: world coordinate Y
"""
wx = self.origin_x + (mx + 0.5) * self.resolution
wy = self.origin_y + (my + 0.5) * self.resolution
return (wx, wy)
def worldToMap(self, wx: float, wy: float) -> tuple[int, int]:
"""
Get the map coordinate XY using world coordinate XY.
Args
----
wx (float) [m]: world coordinate X to get map coordinate
wy (float) [m]: world coordinate Y to get map coordinate
Returns
-------
tuple of int: mx, my
mx (int): map coordinate X
my (int): map coordinate Y
"""
mx = int((wx - self.origin_x) // self.resolution)
my = int((wy - self.origin_y) // self.resolution)
return (mx, my)
def getIndex(self, mx: int, my: int) -> int:
"""
Get the index of the cell using map coordinate XY.
Args
----
mx (int): map coordinate X to get Index
my (int): map coordinate Y to get Index
Returns
-------
int: The index of the cell
"""
return my * self.size_x + mx
navigation2/nav2_simple_commander/nav2_simple_commander/demo_inspection.py
+100
View File
@@ -0,0 +1,100 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from copy import deepcopy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
"""
Basic stock inspection demo. In this demonstration, the expectation
is that there are cameras or RFID sensors mounted on the robots
collecting information about stock quantity and location.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Inspection route, probably read in from a file for a real application
# from either a map or drive and repeat.
inspection_route = [
[3.461, -0.450],
[5.531, -0.450],
[3.461, -2.200],
[5.531, -2.200],
[3.661, -4.121],
[5.431, -4.121],
[3.661, -5.850],
[5.431, -5.800]]
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate
navigator.waitUntilNav2Active()
# Send our route
inspection_points = []
inspection_pose = PoseStamped()
inspection_pose.header.frame_id = 'map'
inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
inspection_pose.pose.orientation.z = 1.0
inspection_pose.pose.orientation.w = 0.0
for pt in inspection_route:
inspection_pose.pose.position.x = pt[0]
inspection_pose.pose.position.y = pt[1]
inspection_points.append(deepcopy(inspection_pose))
navigator.followWaypoints(inspection_points)
# Do something during our route (e.x. AI to analyze stock information or upload to the cloud)
# Simply the current waypoint ID for the demonstation
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Executing current waypoint: ' +
str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Inspection of shelves complete! Returning to start...')
elif result == TaskResult.CANCELED:
print('Inspection of shelving was canceled. Returning to start...')
elif result == TaskResult.FAILED:
print('Inspection of shelving failed! Returning to start...')
# go back to start
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
navigator.goToPose(initial_pose)
while not navigator.isTaskComplete():
pass
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/demo_picking.py
+123
View File
@@ -0,0 +1,123 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
# Shelf positions for picking
shelf_positions = {
'shelf_A': [-3.829, -7.604],
'shelf_B': [-3.791, -3.287],
'shelf_C': [-3.791, 1.254],
'shelf_D': [-3.24, 5.861]}
# Shipping destination for picked products
shipping_destinations = {
'recycling': [-0.205, 7.403],
'pallet_jack7': [-0.073, -8.497],
'conveyer_432': [6.217, 2.153],
'frieght_bay_3': [-6.349, 9.147]}
"""
Basic item picking demo. In this demonstration, the expectation
is that there is a person at the item shelf to put the item on the robot
and at the pallet jack to remove it
(probably with some kind of button for 'got item, robot go do next task').
"""
def main():
# Recieved virtual request for picking item at Shelf A and bring to
# worker at the pallet jack 7 for shipping. This request would
# contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7")
####################
request_item_location = 'shelf_C'
request_destination = 'pallet_jack7'
####################
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate
navigator.waitUntilNav2Active()
shelf_item_pose = PoseStamped()
shelf_item_pose.header.frame_id = 'map'
shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg()
shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0]
shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1]
shelf_item_pose.pose.orientation.z = 1.0
shelf_item_pose.pose.orientation.w = 0.0
print(f'Received request for item picking at {request_item_location}.')
navigator.goToPose(shelf_item_pose)
# Do something during our route
# (e.x. queue up future tasks or detect person for fine-tuned positioning)
# Simply print information for workers on the robot's ETA for the demonstation
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time of arrival at ' + request_item_location +
' for worker: ' + '{0:.0f}'.format(
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+ ' seconds.')
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Got product from ' + request_item_location +
'! Bringing product to shipping destination (' + request_destination + ')...')
shipping_destination = PoseStamped()
shipping_destination.header.frame_id = 'map'
shipping_destination.header.stamp = navigator.get_clock().now().to_msg()
shipping_destination.pose.position.x = shipping_destinations[request_destination][0]
shipping_destination.pose.position.y = shipping_destinations[request_destination][1]
shipping_destination.pose.orientation.z = 1.0
shipping_destination.pose.orientation.w = 0.0
navigator.goToPose(shipping_destination)
elif result == TaskResult.CANCELED:
print(f'Task at {request_item_location} was canceled. Returning to staging point...')
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
navigator.goToPose(initial_pose)
elif result == TaskResult.FAILED:
print(f'Task at {request_item_location} failed!')
exit(-1)
while not navigator.isTaskComplete():
pass
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py
+105
View File
@@ -0,0 +1,105 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic recoveries demo. In this demonstration, the robot navigates
to a dead-end where recoveries such as backup and spin are used
to get out of it.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate
navigator.waitUntilNav2Active()
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = 6.13
goal_pose.pose.position.y = 1.90
goal_pose.pose.orientation.w = 1.0
navigator.goToPose(goal_pose)
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print(
f'Estimated time of arrival to destination is: \
{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9}'
)
# Robot hit a dead end, back it up
print('Robot hit a dead end, backing up...')
navigator.backup(backup_dist=0.5, backup_speed=0.1)
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print(f'Distance traveled: {feedback.distance_traveled}')
# Turn it around
print('Spinning robot around...')
navigator.spin(spin_dist=3.14)
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print(f'Spin angle traveled: {feedback.angular_distance_traveled}')
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Dead end confirmed! Returning to start...')
elif result == TaskResult.CANCELED:
print('Recovery was canceled. Returning to start...')
elif result == TaskResult.FAILED:
print('Recovering from dead end failed! Returning to start...')
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
navigator.goToPose(initial_pose)
while not navigator.isTaskComplete():
pass
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/demo_security.py
+107
View File
@@ -0,0 +1,107 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from copy import deepcopy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic security route patrol demo. In this demonstration, the expectation
is that there are security cameras mounted on the robots recording or being
watched live by security staff.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Security route, probably read in from a file for a real application
# from either a map or drive and repeat.
security_route = [
[1.792, 2.144],
[1.792, -5.44],
[1.792, -9.427],
[-3.665, -9.427],
[-3.665, -4.303],
[-3.665, 2.330],
[-3.665, 9.283]]
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate
navigator.waitUntilNav2Active()
# Do security route until dead
while rclpy.ok():
# Send our route
route_poses = []
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = navigator.get_clock().now().to_msg()
pose.pose.orientation.w = 1.0
for pt in security_route:
pose.pose.position.x = pt[0]
pose.pose.position.y = pt[1]
route_poses.append(deepcopy(pose))
navigator.goThroughPoses(route_poses)
# Do something during our route (e.x. AI detection on camera images for anomalies)
# Simply print ETA for the demonstation
i = 0
while not navigator.isTaskComplete():
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time to complete current route: ' + '{0:.0f}'.format(
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+ ' seconds.')
# Some failure mode, must stop since the robot is clearly stuck
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
print('Navigation has exceeded timeout of 180s, canceling request.')
navigator.cancelTask()
# If at end of route, reverse the route to restart
security_route.reverse()
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Route complete! Restarting...')
elif result == TaskResult.CANCELED:
print('Security route was canceled, exiting.')
exit(1)
elif result == TaskResult.FAILED:
print('Security route failed! Restarting from other side...')
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py
+56
View File
@@ -0,0 +1,56 @@
#! /usr/bin/env python3
# Copyright 2022 Joshua Wallace
#
# 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.
from time import sleep
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy
"""
Basic navigation demo to go to pose.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
navigator.assistedTeleop(time_allowance=20)
while not navigator.isTaskComplete():
# Publish twist commands to be filtered by the assisted teleop action
sleep(0.2)
pass
navigator.lifecycleShutdown()
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/example_follow_path.py
+93
View File
@@ -0,0 +1,93 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
"""
Basic navigation demo to follow a given path after smoothing
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
# Go to our demos first goal pose
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = -3.0
goal_pose.pose.position.y = -2.0
goal_pose.pose.orientation.w = 1.0
# Get the path, smooth it
path = navigator.getPath(initial_pose, goal_pose)
smoothed_path = navigator.smoothPath(path)
# Follow path
navigator.followPath(smoothed_path)
i = 0
while not navigator.isTaskComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i += 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated distance remaining to goal position: ' +
'{0:.3f}'.format(feedback.distance_to_goal) +
'\nCurrent speed of the robot: ' +
'{0:.3f}'.format(feedback.speed))
# Do something depending on the return code
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
+139
View File
@@ -0,0 +1,139 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic navigation demo to go to poses.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Activate navigation, if not autostarted. This should be called after setInitialPose()
# or this will initialize at the origin of the map and update the costmap with bogus readings.
# If autostart, you should `waitUntilNav2Active()` instead.
# navigator.lifecycleStartup()
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
# If desired, you can change or load the map as well
# navigator.changeMap('/path/to/map.yaml')
# You may use the navigator to clear or obtain costmaps
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
# global_costmap = navigator.getGlobalCostmap()
# local_costmap = navigator.getLocalCostmap()
# set our demo's goal poses
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.5
goal_pose1.pose.position.y = 0.55
goal_pose1.pose.orientation.w = 0.707
goal_pose1.pose.orientation.z = 0.707
goal_poses.append(goal_pose1)
# 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
goal_poses.append(goal_pose2)
goal_pose3 = PoseStamped()
goal_pose3.header.frame_id = 'map'
goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
goal_pose3.pose.position.x = -3.6
goal_pose3.pose.position.y = -4.75
goal_pose3.pose.orientation.w = 0.707
goal_pose3.pose.orientation.z = 0.707
goal_poses.append(goal_pose3)
# sanity check a valid path exists
# path = navigator.getPathThroughPoses(initial_pose, goal_poses)
navigator.goThroughPoses(goal_poses)
i = 0
while not navigator.isTaskComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time of arrival: ' + '{0:.0f}'.format(
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+ ' seconds.')
# Some navigation timeout to demo cancellation
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
navigator.cancelTask()
# Some navigation request change to demo preemption
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0):
goal_pose4 = PoseStamped()
goal_pose4.header.frame_id = 'map'
goal_pose4.header.stamp = navigator.get_clock().now().to_msg()
goal_pose4.pose.position.x = -5.0
goal_pose4.pose.position.y = -4.75
goal_pose4.pose.orientation.w = 0.707
goal_pose4.pose.orientation.z = 0.707
navigator.goThroughPoses([goal_pose4])
# Do something depending on the return code
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
+112
View File
@@ -0,0 +1,112 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic navigation demo to go to pose.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Activate navigation, if not autostarted. This should be called after setInitialPose()
# or this will initialize at the origin of the map and update the costmap with bogus readings.
# If autostart, you should `waitUntilNav2Active()` instead.
# navigator.lifecycleStartup()
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
# If desired, you can change or load the map as well
# navigator.changeMap('/path/to/map.yaml')
# You may use the navigator to clear or obtain costmaps
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
# global_costmap = navigator.getGlobalCostmap()
# local_costmap = navigator.getLocalCostmap()
# Go to our demos first goal pose
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = -2.0
goal_pose.pose.position.y = -0.5
goal_pose.pose.orientation.w = 1.0
# sanity check a valid path exists
# path = navigator.getPath(initial_pose, goal_pose)
navigator.goToPose(goal_pose)
i = 0
while not navigator.isTaskComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time of arrival: ' + '{0:.0f}'.format(
Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
+ ' seconds.')
# Some navigation timeout to demo cancellation
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
navigator.cancelTask()
# Some navigation request change to demo preemption
if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0):
goal_pose.pose.position.x = -3.0
navigator.goToPose(goal_pose)
# Do something depending on the return code
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py
+142
View File
@@ -0,0 +1,142 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
"""
Basic navigation demo to go to poses.
"""
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Activate navigation, if not autostarted. This should be called after setInitialPose()
# or this will initialize at the origin of the map and update the costmap with bogus readings.
# If autostart, you should `waitUntilNav2Active()` instead.
# navigator.lifecycleStartup()
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
# If desired, you can change or load the map as well
# navigator.changeMap('/path/to/map.yaml')
# You may use the navigator to clear or obtain costmaps
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
# global_costmap = navigator.getGlobalCostmap()
# local_costmap = navigator.getLocalCostmap()
# 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.5
goal_pose1.pose.position.y = 0.55
goal_pose1.pose.orientation.w = 0.707
goal_pose1.pose.orientation.z = 0.707
goal_poses.append(goal_pose1)
# 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
goal_poses.append(goal_pose2)
goal_pose3 = PoseStamped()
goal_pose3.header.frame_id = 'map'
goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
goal_pose3.pose.position.x = -3.6
goal_pose3.pose.position.y = -4.75
goal_pose3.pose.orientation.w = 0.707
goal_pose3.pose.orientation.z = 0.707
goal_poses.append(goal_pose3)
# sanity check a valid path exists
# path = navigator.getPath(initial_pose, goal_pose1)
nav_start = navigator.get_clock().now()
navigator.followWaypoints(goal_poses)
i = 0
while not navigator.isTaskComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Executing current waypoint: ' +
str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses)))
now = navigator.get_clock().now()
# Some navigation timeout to demo cancellation
if now - nav_start > Duration(seconds=600.0):
navigator.cancelTask()
# Some follow waypoints request change to demo preemption
if now - nav_start > Duration(seconds=35.0):
goal_pose4 = PoseStamped()
goal_pose4.header.frame_id = 'map'
goal_pose4.header.stamp = now.to_msg()
goal_pose4.pose.position.x = -5.0
goal_pose4.pose.position.y = -4.75
goal_pose4.pose.orientation.w = 0.707
goal_pose4.pose.orientation.z = 0.707
goal_poses = [goal_pose4]
nav_start = now
navigator.followWaypoints(goal_poses)
# Do something depending on the return code
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
if __name__ == '__main__':
main()
navigation2/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py
+220
View File
@@ -0,0 +1,220 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
# Copyright 2022 Afif Swaidan
#
# 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.
"""
This is a Python3 API for a Footprint Collision Checker.
It provides the needed methods to manipulate the coordinates
and calculate the cost of a Footprint
"""
from math import cos, sin
from geometry_msgs.msg import Point32, Polygon
from nav2_simple_commander.costmap_2d import PyCostmap2D
from nav2_simple_commander.line_iterator import LineIterator
NO_INFORMATION = 255
LETHAL_OBSTACLE = 254
INSCRIBED_INFLATED_OBSTACLE = 253
MAX_NON_OBSTACLE = 252
FREE_SPACE = 0
class FootprintCollisionChecker:
"""
FootprintCollisionChecker.
FootprintCollisionChecker Class for getting the cost
and checking the collisions of a Footprint
"""
def __init__(self):
"""Initialize the FootprintCollisionChecker Object."""
self.costmap_ = None
pass
def footprintCost(self, footprint: Polygon):
"""
Iterate over all the points in a footprint and check for collision.
Args
----
footprint (Polygon): The footprint to calculate the collision cost for
Returns
-------
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
footprint_cost (float): The maximum cost found in the footprint points
"""
footprint_cost = 0.0
x1 = 0.0
y1 = 0.0
x0, y0 = self.worldToMapValidated(footprint.points[0].x, footprint.points[0].y)
if x0 is None or y0 is None:
return LETHAL_OBSTACLE
xstart = x0
ystart = y0
for i in range(len(footprint.points) - 1):
x1, y1 = self.worldToMapValidated(
footprint.points[i + 1].x, footprint.points[i + 1].y
)
if x1 is None or y1 is None:
return LETHAL_OBSTACLE
footprint_cost = max(float(self.lineCost(x0, x1, y0, y1)), footprint_cost)
x0 = x1
y0 = y1
if footprint_cost == LETHAL_OBSTACLE:
return footprint_cost
return max(float(self.lineCost(xstart, x1, ystart, y1)), footprint_cost)
def lineCost(self, x0, x1, y0, y1, step_size=0.5):
"""
Iterate over all the points along a line and check for collision.
Args
----
x0 (float): Abscissa of the initial point in map coordinates
y0 (float): Ordinate of the initial point in map coordinates
x1 (float): Abscissa of the final point in map coordinates
y1 (float): Ordinate of the final point in map coordinates
step_size (float): Optional, Increments' resolution, defaults to 0.5
Returns
-------
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
line_cost (float): The maximum cost found in the line points
"""
line_cost = 0.0
point_cost = -1.0
line_iterator = LineIterator(x0, y0, x1, y1, step_size)
while line_iterator.isValid():
point_cost = self.pointCost(
int(line_iterator.getX()), int(line_iterator.getY())
)
if point_cost == LETHAL_OBSTACLE:
return point_cost
if line_cost < point_cost:
line_cost = point_cost
line_iterator.advance()
return line_cost
def worldToMapValidated(self, wx: float, wy: float):
"""
Get the map coordinate XY using world coordinate XY.
Args
----
wx (float): world coordinate X
wy (float): world coordinate Y
Returns
-------
None: if coordinates are invalid
tuple of int: mx, my (if coordinates are valid)
mx (int): map coordinate X
my (int): map coordinate Y
"""
if self.costmap_ is None:
raise ValueError(
'Costmap not specified, use setCostmap to specify the costmap first'
)
return self.costmap_.worldToMap(wx, wy)
def pointCost(self, x: int, y: int):
"""
Get the cost of a point in the costmap using map coordinates XY.
Args
----
mx (int): map coordinate X
my (int): map coordinate Y
Returns
-------
np.uint8: cost of a point
"""
if self.costmap_ is None:
raise ValueError(
'Costmap not specified, use setCostmap to specify the costmap first'
)
return self.costmap_.getCostXY(x, y)
def setCostmap(self, costmap: PyCostmap2D):
"""
Specify which costmap to use.
Args
----
costmap (PyCostmap2D): costmap to use in the object's methods
Returns
-------
None
"""
self.costmap_ = costmap
return None
def footprintCostAtPose(self, x: float, y: float, theta: float, footprint: Polygon):
"""
Get the cost of a footprint at a specific Pose in map coordinates.
Args
----
x (float): map coordinate X
y (float): map coordinate Y
theta (float): absolute rotation angle of the footprint
footprint (Polygon): the footprint to calculate its cost at the given Pose
Returns
-------
LETHAL_OBSTACLE (int): If collision was found, 254 will be returned
footprint_cost (float): The maximum cost found in the footprint points
"""
cos_th = cos(theta)
sin_th = sin(theta)
oriented_footprint = Polygon()
for i in range(len(footprint.points)):
new_pt = Point32()
new_pt.x = x + (
footprint.points[i].x * cos_th - footprint.points[i].y * sin_th
)
new_pt.y = y + (
footprint.points[i].x * sin_th + footprint.points[i].y * cos_th
)
oriented_footprint.points.append(new_pt)
return self.footprintCost(oriented_footprint)
navigation2/nav2_simple_commander/nav2_simple_commander/line_iterator.py
+177
View File
@@ -0,0 +1,177 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
# Copyright 2022 Afif Swaidan
#
# 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.
"""
This is a Python3 API for a line iterator.
It provides the ability to iterate
through the points of a line.
"""
from cmath import sqrt
class LineIterator():
"""
LineIterator.
LineIterator Python3 API for iterating along the points of a given line
"""
def __init__(self, x0, y0, x1, y1, step_size=1.0):
"""
Initialize the LineIterator.
Args
----
x0 (float): Abscissa of the initial point
y0 (float): Ordinate of the initial point
x1 (float): Abscissa of the final point
y1 (float): Ordinate of the final point
step_size (float): Optional, Increments' resolution, defaults to 1
Raises
------
TypeError: When one (or more) of the inputs is not a number
ValueError: When step_size is not a positive number
"""
if type(x0) not in [int, float]:
raise TypeError("x0 must be a number (int or float)")
if type(y0) not in [int, float]:
raise TypeError("y0 must be a number (int or float)")
if type(x1) not in [int, float]:
raise TypeError("x1 must be a number (int or float)")
if type(y1) not in [int, float]:
raise TypeError("y1 must be a number (int or float)")
if type(step_size) not in [int, float]:
raise TypeError("step_size must be a number (int or float)")
if step_size <= 0:
raise ValueError("step_size must be a positive number")
self.x0_ = x0
self.y0_ = y0
self.x1_ = x1
self.y1_ = y1
self.x_ = x0
self.y_ = y0
self.step_size_ = step_size
if x1 != x0 and y1 != y0:
self.valid_ = True
self.m_ = (y1-y0)/(x1-x0)
self.b_ = y1 - (self.m_*x1)
elif x1 == x0 and y1 != y0:
self.valid_ = True
elif y1 == y1 and x1 != x0:
self.valid_ = True
self.m_ = (y1-y0)/(x1-x0)
self.b_ = y1 - (self.m_*x1)
else:
self.valid_ = False
raise ValueError(
"Line has zero length (All 4 points have same coordinates)")
def isValid(self):
"""Check if line is valid."""
return self.valid_
def advance(self):
"""Advance to the next point in the line."""
if self.x1_ > self.x0_:
if self.x_ < self.x1_:
self.x_ = round(self.clamp(
self.x_ + self.step_size_, self.x0_, self.x1_), 5)
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
else:
self.valid_ = False
elif self.x1_ < self.x0_:
if self.x_ > self.x1_:
self.x_ = round(self.clamp(
self.x_ - self.step_size_, self.x1_, self.x0_), 5)
self.y_ = round(self.m_ * self.x_ + self.b_, 5)
else:
self.valid_ = False
else:
if self.y1_ > self.y0_:
if self.y_ < self.y1_:
self.y_ = round(self.clamp(
self.y_ + self.step_size_, self.y0_, self.y1_), 5)
else:
self.valid_ = False
elif self.y1_ < self.y0_:
if self.y_ > self.y1_:
self.y_ = round(self.clamp(
self.y_ - self.step_size_, self.y1_, self.y0_), 5)
else:
self.valid_ = False
else:
self.valid_ = False
def getX(self):
"""Get the abscissa of the current point."""
return self.x_
def getY(self):
"""Get the ordinate of the current point."""
return self.y_
def getX0(self):
"""Get the abscissa of the initial point."""
return self.x0_
def getY0(self):
"""Get the ordinate of the intial point."""
return self.y0_
def getX1(self):
"""Get the abscissa of the final point."""
return self.x1_
def getY1(self):
"""Get the ordinate of the final point."""
return self.y1_
def get_line_length(self):
"""Get the length of the line."""
return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2))
def clamp(self, n, min_n, max_n):
"""
Clamp n to be between min_n and max_n.
Args
----
n (float): input value
min_n (float): minimum value
max_n (float): maximum value
Returns
-------
n (float): input value clamped between given min and max
"""
if n < min_n:
return min_n
elif n > max_n:
return max_n
else:
return n
navigation2/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
+593
View File
@@ -0,0 +1,593 @@
#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# 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.
from enum import Enum
import time
from action_msgs.msg import GoalStatus
from builtin_interfaces.msg import Duration
from geometry_msgs.msg import Point
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped
from lifecycle_msgs.srv import GetState
from nav2_msgs.action import AssistedTeleop, BackUp, Spin
from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose
from nav2_msgs.action import SmoothPath
from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes
import rclpy
from rclpy.action import ActionClient
from rclpy.duration import Duration as rclpyDuration
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
class TaskResult(Enum):
UNKNOWN = 0
SUCCEEDED = 1
CANCELED = 2
FAILED = 3
class BasicNavigator(Node):
def __init__(self, node_name='basic_navigator', namespace=''):
super().__init__(node_name=node_name, namespace=namespace)
self.initial_pose = PoseStamped()
self.initial_pose.header.frame_id = 'map'
self.goal_handle = None
self.result_future = None
self.feedback = None
self.status = None
amcl_pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1)
self.initial_pose_received = False
self.nav_through_poses_client = ActionClient(self,
NavigateThroughPoses,
'navigate_through_poses')
self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
self.follow_path_client = ActionClient(self, FollowPath, 'follow_path')
self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose,
'compute_path_to_pose')
self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses,
'compute_path_through_poses')
self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path')
self.spin_client = ActionClient(self, Spin, 'spin')
self.backup_client = ActionClient(self, BackUp, 'backup')
self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop')
self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
'amcl_pose',
self._amclPoseCallback,
amcl_pose_qos)
self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
'initialpose',
10)
self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map')
self.clear_costmap_global_srv = self.create_client(
ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap')
self.clear_costmap_local_srv = self.create_client(
ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap')
self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap')
self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap')
def destroyNode(self):
self.destroy_node()
def destroy_node(self):
self.nav_through_poses_client.destroy()
self.nav_to_pose_client.destroy()
self.follow_waypoints_client.destroy()
self.follow_path_client.destroy()
self.compute_path_to_pose_client.destroy()
self.compute_path_through_poses_client.destroy()
self.smoother_client.destroy()
self.spin_client.destroy()
self.backup_client.destroy()
super().destroy_node()
def setInitialPose(self, initial_pose):
"""Set the initial pose to the localization system."""
self.initial_pose_received = False
self.initial_pose = initial_pose
self._setInitialPose()
def goThroughPoses(self, poses, behavior_tree=''):
"""Send a `NavThroughPoses` action request."""
self.debug("Waiting for 'NavigateThroughPoses' action server")
while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0):
self.info("'NavigateThroughPoses' action server not available, waiting...")
goal_msg = NavigateThroughPoses.Goal()
goal_msg.poses = poses
goal_msg.behavior_tree = behavior_tree
self.info(f'Navigating with {len(goal_msg.poses)} goals....')
send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg,
self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error(f'Goal with {len(poses)} poses was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def goToPose(self, pose, behavior_tree=''):
"""Send a `NavToPose` action request."""
self.debug("Waiting for 'NavigateToPose' action server")
while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
self.info("'NavigateToPose' action server not available, waiting...")
goal_msg = NavigateToPose.Goal()
goal_msg.pose = pose
goal_msg.behavior_tree = behavior_tree
self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' +
str(pose.pose.position.y) + '...')
send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg,
self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Goal to ' + str(pose.pose.position.x) + ' ' +
str(pose.pose.position.y) + ' was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def followWaypoints(self, poses):
"""Send a `FollowWaypoints` action request."""
self.debug("Waiting for 'FollowWaypoints' action server")
while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0):
self.info("'FollowWaypoints' action server not available, waiting...")
goal_msg = FollowWaypoints.Goal()
goal_msg.poses = poses
self.info(f'Following {len(goal_msg.poses)} goals....')
send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg,
self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error(f'Following {len(poses)} waypoints request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def spin(self, spin_dist=1.57, time_allowance=10):
self.debug("Waiting for 'Spin' action server")
while not self.spin_client.wait_for_server(timeout_sec=1.0):
self.info("'Spin' action server not available, waiting...")
goal_msg = Spin.Goal()
goal_msg.target_yaw = spin_dist
goal_msg.time_allowance = Duration(sec=time_allowance)
self.info(f'Spinning to angle {goal_msg.target_yaw}....')
send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Spin request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
self.debug("Waiting for 'Backup' action server")
while not self.backup_client.wait_for_server(timeout_sec=1.0):
self.info("'Backup' action server not available, waiting...")
goal_msg = BackUp.Goal()
goal_msg.target = Point(x=float(backup_dist))
goal_msg.speed = backup_speed
goal_msg.time_allowance = Duration(sec=time_allowance)
self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Backup request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def assistedTeleop(self, time_allowance=30):
self.debug("Wainting for 'assisted_teleop' action server")
while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0):
self.info("'assisted_teleop' action server not available, waiting...")
goal_msg = AssistedTeleop.Goal()
goal_msg.time_allowance = Duration(sec=time_allowance)
self.info("Running 'assisted_teleop'....")
send_goal_future = \
self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Assisted Teleop request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def followPath(self, path, controller_id='', goal_checker_id=''):
"""Send a `FollowPath` action request."""
self.debug("Waiting for 'FollowPath' action server")
while not self.follow_path_client.wait_for_server(timeout_sec=1.0):
self.info("'FollowPath' action server not available, waiting...")
goal_msg = FollowPath.Goal()
goal_msg.path = path
goal_msg.controller_id = controller_id
goal_msg.goal_checker_id = goal_checker_id
self.info('Executing path...')
send_goal_future = self.follow_path_client.send_goal_async(goal_msg,
self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Follow path was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def cancelTask(self):
"""Cancel pending task request of any type."""
self.info('Canceling current task.')
if self.result_future:
future = self.goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(self, future)
return
def isTaskComplete(self):
"""Check if the task request of any type is complete yet."""
if not self.result_future:
# task was cancelled or completed
return True
rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10)
if self.result_future.result():
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.debug(f'Task with failed with status code: {self.status}')
return True
else:
# Timed out, still processing, not complete yet
return False
self.debug('Task succeeded!')
return True
def getFeedback(self):
"""Get the pending action feedback message."""
return self.feedback
def getResult(self):
"""Get the pending action result message."""
if self.status == GoalStatus.STATUS_SUCCEEDED:
return TaskResult.SUCCEEDED
elif self.status == GoalStatus.STATUS_ABORTED:
return TaskResult.FAILED
elif self.status == GoalStatus.STATUS_CANCELED:
return TaskResult.CANCELED
else:
return TaskResult.UNKNOWN
def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
"""Block until the full navigation system is up and running."""
self._waitForNodeToActivate(localizer)
if localizer == 'amcl':
self._waitForInitialPose()
self._waitForNodeToActivate(navigator)
self.info('Nav2 is ready for use!')
return
def _getPathImpl(self, start, goal, planner_id='', use_start=False):
"""
Send a `ComputePathToPose` action request.
Internal implementation to get the full result, not just the path.
"""
self.debug("Waiting for 'ComputePathToPose' action server")
while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0):
self.info("'ComputePathToPose' action server not available, waiting...")
goal_msg = ComputePathToPose.Goal()
goal_msg.start = start
goal_msg.goal = goal
goal_msg.planner_id = planner_id
goal_msg.use_start = use_start
self.info('Getting path...')
send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Get path was rejected!')
return None
self.result_future = self.goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, self.result_future)
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.warn(f'Getting path failed with status code: {self.status}')
return None
return self.result_future.result().result
def getPath(self, start, goal, planner_id='', use_start=False):
"""Send a `ComputePathToPose` action request."""
rtn = self._getPathImpl(start, goal, planner_id, use_start)
if not rtn:
return None
else:
return rtn.path
def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
"""Send a `ComputePathThroughPoses` action request."""
self.debug("Waiting for 'ComputePathThroughPoses' action server")
while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0):
self.info("'ComputePathThroughPoses' action server not available, waiting...")
goal_msg = ComputePathThroughPoses.Goal()
goal_msg.start = start
goal_msg.goals = goals
goal_msg.planner_id = planner_id
goal_msg.use_start = use_start
self.info('Getting path...')
send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Get path was rejected!')
return None
self.result_future = self.goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, self.result_future)
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.warn(f'Getting path failed with status code: {self.status}')
return None
return self.result_future.result().result.path
def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
"""
Send a `SmoothPath` action request.
Internal implementation to get the full result, not just the path.
"""
self.debug("Waiting for 'SmoothPath' action server")
while not self.smoother_client.wait_for_server(timeout_sec=1.0):
self.info("'SmoothPath' action server not available, waiting...")
goal_msg = SmoothPath.Goal()
goal_msg.path = path
goal_msg.max_smoothing_duration = rclpyDuration(seconds=max_duration).to_msg()
goal_msg.smoother_id = smoother_id
goal_msg.check_for_collisions = check_for_collision
self.info('Smoothing path...')
send_goal_future = self.smoother_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Smooth path was rejected!')
return None
self.result_future = self.goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, self.result_future)
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.warn(f'Getting path failed with status code: {self.status}')
return None
return self.result_future.result().result
def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
"""Send a `SmoothPath` action request."""
rtn = self._smoothPathImpl(
path, smoother_id, max_duration, check_for_collision)
if not rtn:
return None
else:
return rtn.path
def changeMap(self, map_filepath):
"""Change the current static map in the map server."""
while not self.change_maps_srv.wait_for_service(timeout_sec=1.0):
self.info('change map service not available, waiting...')
req = LoadMap.Request()
req.map_url = map_filepath
future = self.change_maps_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
status = future.result().result
if status != LoadMap.Response().RESULT_SUCCESS:
self.error('Change map request failed!')
else:
self.info('Change map request was successful!')
return
def clearAllCostmaps(self):
"""Clear all costmaps."""
self.clearLocalCostmap()
self.clearGlobalCostmap()
return
def clearLocalCostmap(self):
"""Clear local costmap."""
while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0):
self.info('Clear local costmaps service not available, waiting...')
req = ClearEntireCostmap.Request()
future = self.clear_costmap_local_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
return
def clearGlobalCostmap(self):
"""Clear global costmap."""
while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0):
self.info('Clear global costmaps service not available, waiting...')
req = ClearEntireCostmap.Request()
future = self.clear_costmap_global_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
return
def getGlobalCostmap(self):
"""Get the global costmap."""
while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):
self.info('Get global costmaps service not available, waiting...')
req = GetCostmap.Request()
future = self.get_costmap_global_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
return future.result().map
def getLocalCostmap(self):
"""Get the local costmap."""
while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0):
self.info('Get local costmaps service not available, waiting...')
req = GetCostmap.Request()
future = self.get_costmap_local_srv.call_async(req)
rclpy.spin_until_future_complete(self, future)
return future.result().map
def lifecycleStartup(self):
"""Startup nav2 lifecycle system."""
self.info('Starting up lifecycle nodes based on lifecycle_manager.')
for srv_name, srv_type in self.get_service_names_and_types():
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
self.info(f'Starting up {srv_name}')
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info(f'{srv_name} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().STARTUP
future = mgr_client.call_async(req)
# starting up requires a full map->odom->base_link TF tree
# so if we're not successful, try forwarding the initial pose
while True:
rclpy.spin_until_future_complete(self, future, timeout_sec=0.10)
if not future:
self._waitForInitialPose()
else:
break
self.info('Nav2 is ready for use!')
return
def lifecycleShutdown(self):
"""Shutdown nav2 lifecycle system."""
self.info('Shutting down lifecycle nodes based on lifecycle_manager.')
for srv_name, srv_type in self.get_service_names_and_types():
if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
self.info(f'Shutting down {srv_name}')
mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
while not mgr_client.wait_for_service(timeout_sec=1.0):
self.info(f'{srv_name} service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
future = mgr_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
future.result()
return
def _waitForNodeToActivate(self, node_name):
# Waits for the node within the tester namespace to become active
self.debug(f'Waiting for {node_name} to become active..')
node_service = f'{node_name}/get_state'
state_client = self.create_client(GetState, node_service)
while not state_client.wait_for_service(timeout_sec=1.0):
self.info(f'{node_service} service not available, waiting...')
req = GetState.Request()
state = 'unknown'
while state != 'active':
self.debug(f'Getting {node_name} state...')
future = state_client.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
state = future.result().current_state.label
self.debug(f'Result of get_state: {state}')
time.sleep(2)
return
def _waitForInitialPose(self):
while not self.initial_pose_received:
self.info('Setting initial pose')
self._setInitialPose()
self.info('Waiting for amcl_pose to be received')
rclpy.spin_once(self, timeout_sec=1.0)
return
def _amclPoseCallback(self, msg):
self.debug('Received amcl pose')
self.initial_pose_received = True
return
def _feedbackCallback(self, msg):
self.debug('Received action feedback message')
self.feedback = msg.feedback
return
def _setInitialPose(self):
msg = PoseWithCovarianceStamped()
msg.pose.pose = self.initial_pose.pose
msg.header.frame_id = self.initial_pose.header.frame_id
msg.header.stamp = self.initial_pose.header.stamp
self.info('Publishing Initial Pose')
self.initial_pose_pub.publish(msg)
return
def info(self, msg):
self.get_logger().info(msg)
return
def warn(self, msg):
self.get_logger().warn(msg)
return
def error(self, msg):
self.get_logger().error(msg)
return
def debug(self, msg):
self.get_logger().debug(msg)
return