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.isNavComplete():
################################################
#
# 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.cancelNav()
# 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 == NavigationResult.SUCCEEDED:
print('Goal succeeded!')
elif result == NavigationResult.CANCELED:
print('Goal was canceled!')
elif result == NavigationResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
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
# Sanity check a valid path exists
path = navigator.getPath(initial_pose, goal_pose)
# Follow path
navigator.followPath(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)
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.isNavComplete():
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.cancelNav()
# If at end of route, reverse the route to restart
security_route.reverse()
result = navigator.getResult()
if result == NavigationResult.SUCCEEDED:
print('Route complete! Restarting...')
elif result == NavigationResult.CANCELED:
print('Security route was canceled, exiting.')
exit(1)
elif result == NavigationResult.FAILED:
print('Security route failed! Restarting from other side...')
exit(0)
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.isNavComplete():
################################################
#
# 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.cancelNav()
# 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 == NavigationResult.SUCCEEDED:
print('Goal succeeded!')
elif result == NavigationResult.CANCELED:
print('Goal was canceled!')
elif result == NavigationResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
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('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.isNavComplete():
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 == NavigationResult.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 == NavigationResult.CANCELED:
print('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 == NavigationResult.FAILED:
print('Task at ' + request_item_location + ' failed!')
exit(-1)
while not navigator.isNavComplete():
pass
exit(0)
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.isNavComplete():
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 == NavigationResult.SUCCEEDED:
print('Inspection of shelves complete! Returning to start...')
elif result == NavigationResult.CANCELED:
print('Inspection of shelving was canceled. Returning to start...')
exit(1)
elif result == NavigationResult.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.isNavComplete():
pass
exit(0)
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)
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our experiment'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 = 1.0
initial_pose.pose.position.y = 1.0
initial_pose.pose.orientation.z = 0.0
initial_pose.pose.orientation.w = 1.0
navigator.setInitialPose(initial_pose)
# Wait for navigation to fully activate
navigator.waitUntilNav2Active()
# Get the costmap for start/goal validation
costmap_msg = navigator.getGlobalCostmap()
costmap = np.asarray(costmap_msg.data)
costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x)
planners = ['NavFn', 'Smac2d', 'SmacLattice', 'SmacHybrid']
max_cost = 210
side_buffer = 100
time_stamp = navigator.get_clock().now().to_msg()
results = []
seed(33)
random_pairs = 100
res = costmap_msg.metadata.resolution
for i in range(random_pairs):
print("Cycle: ", i, "out of: ", random_pairs)
start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res)
goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp,
res)
print("Start", start)
print("Goal", goal)
result = getPlannerResults(navigator, start, goal, planners)
if len(result) == len(planners):
results.append(result)
else:
print("One of the planners was invalid")
print("Write Results...")
nav2_planner_metrics_dir = get_package_share_directory(
'nav2_planner_metrics')
with open(os.path.join(nav2_planner_metrics_dir, 'results.pickle'),
'wb') as f:
pickle.dump(results, f, pickle.HIGHEST_PROTOCOL)
with open(os.path.join(nav2_planner_metrics_dir, 'costmap.pickle'),
'wb') as f:
pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL)
with open(os.path.join(nav2_planner_metrics_dir, 'planners.pickle'),
'wb') as f:
pickle.dump(planners, f, pickle.HIGHEST_PROTOCOL)
print("Write Complete")
navigator.lifecycleShutdown()
exit(0)