Nav2 Navigation Code Examples
1. Basic Nav2 Navigation Node
# Example: Basic Nav2 Navigation Node
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from std_msgs.msg import String
import time
class BasicNav2Node(Node):
def __init__(self):
super().__init__('basic_nav2_node')
# Create action client for navigation
self.nav_to_pose_client = ActionClient(
self,
NavigateToPose,
'navigate_to_pose'
)
# Create publisher for navigation status
self.status_pub = self.create_publisher(String, 'nav2_status', 10)
# Timer for navigation tasks
self.nav_timer = self.create_timer(10.0, self.navigation_task)
self.get_logger().info('Basic Nav2 Node initialized')
def navigation_task(self):
"""Example navigation task"""
# Check if action server is available
if not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
self.get_logger().error('NavigateToPose action server not available')
return
# Create goal message
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
# Set a simple goal position (adjust coordinates as needed)
goal_msg.pose.pose.position.x = 1.0
goal_msg.pose.pose.position.y = 1.0
goal_msg.pose.pose.orientation.w = 1.0
# Send navigation goal
self.get_logger().info(f'Sending navigation goal to ({goal_msg.pose.pose.position.x}, {goal_msg.pose.pose.position.y})')
future = self.nav_to_pose_client.send_goal_async(
goal_msg,
feedback_callback=self.feedback_callback
)
future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
"""Handle goal response"""
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected')
return
self.get_logger().info('Goal accepted')
result_future = goal_handle.get_result_async()
result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
"""Handle navigation result"""
result = future.result().result
status = String()
status.data = f'Navigation completed with status: {result}'
self.status_pub.publish(status)
def feedback_callback(self, feedback_msg):
"""Handle navigation feedback"""
feedback = feedback_msg.feedback
self.get_logger().info(f'Navigation feedback: {feedback}')
def main(args=None):
rclpy.init(args=args)
nav_node = BasicNav2Node()
try:
rclpy.spin(nav_node)
except KeyboardInterrupt:
pass
finally:
nav_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
2. Nav2 Path Planning Node
# Example: Nav2 Path Planning Node
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped, Point
from nav2_msgs.action import ComputePathToPose
from nav_msgs.msg import Path
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np
class Nav2PathPlannerNode(Node):
def __init__(self):
super().__init__('nav2_path_planner')
# Create action client for path planning
self.compute_path_client = ActionClient(
self,
ComputePathToPose,
'compute_path_to_pose'
)
# Create publisher for planned path
self.path_pub = self.create_publisher(Path, 'plan', 10)
# Create publisher for path visualization
self.vis_pub = self.create_publisher(MarkerArray, 'path_visualization', 10)
# Timer for path planning tasks
self.planning_timer = self.create_timer(5.0, self.plan_path_task)
self.get_logger().info('Nav2 Path Planner Node initialized')
def plan_path_task(self):
"""Example path planning task"""
# Check if action server is available
if not self.compute_path_client.wait_for_server(timeout_sec=1.0):
self.get_logger().error('ComputePathToPose action server not available')
return
# Create goal message for path planning
goal_msg = ComputePathToPose.Goal()
goal_msg.goal.header.frame_id = 'map'
goal_msg.goal.header.stamp = self.get_clock().now().to_msg()
# Set a simple goal position
goal_msg.goal.pose.position.x = 2.0
goal_msg.goal.pose.position.y = 2.0
goal_msg.goal.pose.orientation.w = 1.0
# Send path planning goal
self.get_logger().info(f'Planning path to ({goal_msg.goal.pose.position.x}, {goal_msg.goal.pose.position.y})')
future = self.compute_path_client.send_goal_async(goal_msg)
future.add_done_callback(self.path_plan_response_callback)
def path_plan_response_callback(self, future):
"""Handle path planning response"""
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Path planning goal rejected')
return
self.get_logger().info('Path planning goal accepted')
result_future = goal_handle.get_result_async()
result_future.add_done_callback(self.path_plan_result_callback)
def path_plan_result_callback(self, future):
"""Handle path planning result"""
result = future.result().result
path = result.path
# Publish the planned path
self.path_pub.publish(path)
# Visualize the path
self.visualize_path(path)
self.get_logger().info(f'Path planning completed with {len(path.poses)} waypoints')
def visualize_path(self, path):
"""Visualize the planned path in RViz"""
marker_array = MarkerArray()
# Create marker for the path
path_marker = Marker()
path_marker.header.frame_id = 'map'
path_marker.header.stamp = self.get_clock().now().to_msg()
path_marker.ns = 'planned_path'
path_marker.id = 0
path_marker.type = Marker.LINE_STRIP
path_marker.action = Marker.ADD
# Set scale
path_marker.scale.x = 0.05 # Line width
# Set color (green)
path_marker.color.r = 0.0
path_marker.color.g = 1.0
path_marker.color.b = 0.0
path_marker.color.a = 1.0
# Add path points to marker
for pose_stamped in path.poses:
point = Point()
point.x = pose_stamped.pose.position.x
point.y = pose_stamped.pose.position.y
point.z = pose_stamped.pose.position.z
path_marker.points.append(point)
marker_array.markers.append(path_marker)
# Create markers for path waypoints
for i, pose_stamped in enumerate(path.poses[::10]): # Every 10th point
waypoint_marker = Marker()
waypoint_marker.header.frame_id = 'map'
waypoint_marker.header.stamp = self.get_clock().now().to_msg()
waypoint_marker.ns = 'waypoints'
waypoint_marker.id = i
waypoint_marker.type = Marker.SPHERE
waypoint_marker.action = Marker.ADD
# Set position
waypoint_marker.pose = pose_stamped.pose
# Set scale
waypoint_marker.scale.x = 0.1
waypoint_marker.scale.y = 0.1
waypoint_marker.scale.z = 0.1
# Set color (blue)
waypoint_marker.color.r = 0.0
waypoint_marker.color.g = 0.0
waypoint_marker.color.b = 1.0
waypoint_marker.color.a = 1.0
marker_array.markers.append(waypoint_marker)
self.vis_pub.publish(marker_array)
def main(args=None):
rclpy.init(args=args)
planner = Nav2PathPlannerNode()
try:
rclpy.spin(planner)
except KeyboardInterrupt:
pass
finally:
planner.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
3. Nav2 Controller Node
# Example: Nav2 Controller Node
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import math
class Nav2ControllerNode(Node):
def __init__(self):
super().__init__('nav2_controller')
# Create publisher for velocity commands
self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
# Create subscription for odometry
self.odom_sub = self.create_subscription(
Odometry,
'odom',
self.odom_callback,
10
)
# TF buffer for transforms
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# Robot state
self.current_pose = None
self.target_pose = PoseStamped()
self.target_set = False
# Control parameters
self.linear_speed = 0.2
self.angular_speed = 0.5
self.arrival_threshold = 0.2
self.rotation_threshold = 0.1
# Timer for control loop
self.control_timer = self.create_timer(0.05, self.control_loop) # 20 Hz
self.get_logger().info('Nav2 Controller Node initialized')
def odom_callback(self, msg):
"""Update current pose from odometry"""
self.current_pose = msg.pose.pose
def set_target(self, x, y):
"""Set a new target position"""
self.target_pose.pose.position.x = x
self.target_pose.pose.position.y = y
self.target_set = True
self.get_logger().info(f'Target set to ({x}, {y})')
def control_loop(self):
"""Main control loop for navigation"""
if self.current_pose is None or not self.target_set:
return
# Calculate distance to target
dx = self.target_pose.pose.position.x - self.current_pose.position.x
dy = self.target_pose.pose.position.y - self.current_pose.position.y
distance = math.sqrt(dx**2 + dy**2)
# Check if we've reached the target
if distance < self.arrival_threshold:
self.stop_robot()
self.get_logger().info('Reached target position')
self.target_set = False
return
# Calculate desired heading
desired_theta = math.atan2(dy, dx)
# Get current orientation (simplified - in practice use quaternions properly)
current_theta = 2 * math.atan2(
self.current_pose.orientation.z,
self.current_pose.orientation.w
)
# Calculate heading error
heading_error = desired_theta - current_theta
# Normalize angle to [-pi, pi]
while heading_error > math.pi:
heading_error -= 2 * math.pi
while heading_error < -math.pi:
heading_error += 2 * math.pi
# Create velocity command
cmd = Twist()
# If not aligned with target, rotate first
if abs(heading_error) > self.rotation_threshold:
cmd.angular.z = self.angular_speed if heading_error > 0 else -self.angular_speed
else:
# Move forward toward target
cmd.linear.x = min(self.linear_speed, distance) # Slow down when close
cmd.angular.z = self.angular_speed * heading_error # Fine adjustment
self.cmd_vel_pub.publish(cmd)
def stop_robot(self):
"""Stop the robot"""
cmd = Twist()
cmd.linear.x = 0.0
cmd.angular.z = 0.0
self.cmd_vel_pub.publish(cmd)
def main(args=None):
rclpy.init(args=args)
controller = Nav2ControllerNode()
# Set a target position
controller.set_target(1.0, 1.0)
try:
rclpy.spin(controller)
except KeyboardInterrupt:
pass
finally:
controller.stop_robot() # Stop robot before shutting down
controller.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4. Nav2 Costmap Processor
# Example: Nav2 Costmap Processor
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker, MarkerArray
from sensor_msgs.msg import LaserScan
import numpy as np
class Nav2CostmapProcessorNode(Node):
def __init__(self):
super().__init__('nav2_costmap_processor')
# Create subscription for costmap
self.costmap_sub = self.create_subscription(
OccupancyGrid,
'global_costmap/costmap',
self.costmap_callback,
10
)
# Create subscription for local costmap
self.local_costmap_sub = self.create_subscription(
OccupancyGrid,
'local_costmap/costmap',
self.local_costmap_callback,
10
)
# Create publisher for costmap visualization
self.vis_pub = self.create_publisher(MarkerArray, 'costmap_visualization', 10)
# Create publisher for safety status
self.safety_pub = self.create_publisher(
rclpy.qos.QoSProfile(depth=10),
'safety_status',
'std_msgs/msg/String'
)
# Initialize costmap data
self.global_costmap_data = None
self.local_costmap_data = None
self.costmap_info = None
# Safety parameters
self.declare_parameters(
namespace='',
parameters=[
('safety_threshold', 70), # Occupancy grid values: 0=free, 100=occupied
('clearance_distance', 0.5), # Meters
('robot_radius', 0.3) # Robot radius in meters
]
)
self.get_logger().info('Nav2 Costmap Processor Node initialized')
def costmap_callback(self, msg):
"""Process incoming global costmap data"""
self.global_costmap_data = np.array(msg.data).reshape((msg.info.height, msg.info.width))
self.costmap_info = msg.info
# Process and visualize costmap
self.process_global_costmap()
self.visualize_costmap()
def local_costmap_callback(self, msg):
"""Process incoming local costmap data"""
self.local_costmap_data = np.array(msg.data).reshape((msg.info.height, msg.info.width))
self.costmap_info = msg.info
# Process local costmap
self.process_local_costmap()
def process_global_costmap(self):
"""Process global costmap data"""
if self.global_costmap_data is None:
return
# Get parameters
safety_threshold = self.get_parameter('safety_threshold').get_parameter_value().integer_value
# Find safe paths (areas with low cost)
safe_threshold = safety_threshold // 2
safe_areas = np.where(self.global_costmap_data < safe_threshold)
safe_count = len(safe_areas[0])
# Find obstacles
obstacle_threshold = safety_threshold
obstacles = np.where(self.global_costmap_data > obstacle_threshold)
obstacle_count = len(obstacles[0])
self.get_logger().info(f'Global costmap: {safe_count} safe cells, {obstacle_count} obstacle cells')
def process_local_costmap(self):
"""Process local costmap data for immediate safety"""
if self.local_costmap_data is None:
return
# Get parameters
safety_threshold = self.get_parameter('safety_threshold').get_parameter_value().integer_value
robot_radius = self.get_parameter('robot_radius').get_parameter_value().double_value
# Check for obstacles in the immediate vicinity
center_x = self.local_costmap_data.shape[1] // 2
center_y = self.local_costmap_data.shape[0] // 2
radius_cells = int(robot_radius / self.costmap_info.resolution)
# Extract region around robot
start_x = max(0, center_x - radius_cells)
end_x = min(self.local_costmap_data.shape[1], center_x + radius_cells)
start_y = max(0, center_y - radius_cells)
end_y = min(self.local_costmap_data.shape[0], center_y + radius_cells)
local_region = self.local_costmap_data[start_y:end_y, start_x:end_x]
max_obstacle = np.max(local_region)
# Publish safety status
from std_msgs.msg import String
safety_msg = String()
if max_obstacle > safety_threshold:
safety_msg.data = 'OBSTACLE_DETECTED'
self.get_logger().warn('Obstacle detected in local vicinity!')
else:
safety_msg.data = 'SAFE'
# Use the correct publisher
safety_publisher = self.create_publisher(String, 'safety_status', 10)
safety_publisher.publish(safety_msg)
def visualize_costmap(self):
"""Visualize costmap in RViz"""
if self.global_costmap_data is None or self.costmap_info is None:
return
marker_array = MarkerArray()
# Create markers for costmap cells (sample every 5th cell for performance)
for y in range(0, self.costmap_info.height, 5):
for x in range(0, self.costmap_info.width, 5):
cost = self.global_costmap_data[y, x]
if cost > 0: # Only visualize non-free cells
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = self.get_clock().now().to_msg()
marker.ns = 'costmap_cells'
marker.id = y * self.costmap_info.width + x
marker.type = Marker.CUBE
marker.action = Marker.ADD
# Convert grid coordinates to world coordinates
world_x = self.costmap_info.origin.position.x + x * self.costmap_info.resolution
world_y = self.costmap_info.origin.position.y + y * self.costmap_info.resolution
world_z = self.costmap_info.origin.position.z
marker.pose.position.x = world_x
marker.pose.position.y = world_y
marker.pose.position.z = world_z
marker.pose.orientation.w = 1.0
# Set scale based on resolution
marker.scale.x = self.costmap_info.resolution
marker.scale.y = self.costmap_info.resolution
marker.scale.z = 0.1 # Height of visualization
# Set color based on cost value
if cost > 80: # Definitely occupied
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
elif cost > 50: # Probably occupied
marker.color.r = 1.0
marker.color.g = 0.5
marker.color.b = 0.0
elif cost > 20: # Inflated obstacle
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
else: # Free space with some cost
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.7 # Transparency
marker_array.markers.append(marker)
self.vis_pub.publish(marker_array)
def main(args=None):
rclpy.init(args=args)
processor = Nav2CostmapProcessorNode()
try:
rclpy.spin(processor)
except KeyboardInterrupt:
pass
finally:
processor.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
5. Nav2 Behavior Tree Custom Nodes
# Example: Custom Nav2 Behavior Tree Nodes
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from std_msgs.msg import String
from py_trees import common
from py_trees.behaviour import Behaviour
from py_trees.decorators import Retry
from py_trees.composites import Sequence, Selector
import py_trees
import time
class Nav2BehaviorTreeNode(Node):
def __init__(self):
super().__init__('nav2_behavior_tree')
# Create action client for navigation
self.nav_to_pose_client = ActionClient(
self,
NavigateToPose,
'navigate_to_pose'
)
# Create publisher for BT status
self.bt_status_pub = self.create_publisher(String, 'bt_status', 10)
# Timer for behavior tree execution
self.bt_timer = self.create_timer(0.1, self.tick_behavior_tree)
# Initialize behavior tree
self.setup_behavior_tree()
self.get_logger().info('Nav2 Behavior Tree Node initialized')
def setup_behavior_tree(self):
"""Setup the behavior tree structure"""
# Create the behavior tree
self.root = Sequence(name="NavigateWithRecovery")
# Add behaviors to the tree
compute_path = ComputePathToPoseBehavior("ComputePath")
follow_path = FollowPathBehavior("FollowPath")
recovery = RecoveryBehavior("Recovery")
# Add to root sequence
self.root.add_child(compute_path)
self.root.add_child(follow_path)
# Recovery sequence
recovery_sequence = Sequence(name="RecoverySequence")
recovery_sequence.add_child(recovery)
recovery_sequence.add_child(compute_path)
recovery_sequence.add_child(follow_path)
# Use selector for recovery - if follow_path fails, try recovery
selector = Selector(name="NavigateOrRecover")
selector.add_child(self.root)
selector.add_child(recovery_sequence)
self.behavior_tree = py_trees.trees.BehaviourTree(selector)
# Setup the tree
self.behavior_tree.setup(timeout=15)
def tick_behavior_tree(self):
"""Tick the behavior tree"""
try:
# Tick the behavior tree
self.behavior_tree.tick()
# Publish status
status_msg = String()
status_msg.data = f"BT Status: {self.behavior_tree.root.status}"
self.bt_status_pub.publish(status_msg)
except Exception as e:
self.get_logger().error(f'Error ticking behavior tree: {str(e)}')
class ComputePathToPoseBehavior(Behaviour):
def __init__(self, name):
super(ComputePathToPoseBehavior, self).__init__(name)
self.node = None
self.client = None
self.goal_sent = False
self.result_received = False
def setup(self, node):
self.node = node
self.client = ActionClient(
node,
NavigateToPose,
'navigate_to_pose'
)
def initialise(self):
self.goal_sent = False
self.result_received = False
self.node.get_logger().info(f"{self.name} initialized")
def update(self):
if not self.goal_sent:
# Check if action server is available
if not self.client.wait_for_server(timeout_sec=1.0):
self.node.get_logger().error('NavigateToPose action server not available')
return common.Status.FAILURE
# Create goal message
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.header.stamp = self.node.get_clock().now().to_msg()
goal_msg.pose.pose.position.x = 1.0 # Example target
goal_msg.pose.pose.position.y = 1.0
goal_msg.pose.pose.orientation.w = 1.0
# Send navigation goal
future = self.client.send_goal_async(goal_msg)
future.add_done_callback(self.goal_response_callback)
self.goal_sent = True
return common.Status.RUNNING
if self.result_received:
return common.Status.SUCCESS
return common.Status.RUNNING
def goal_response_callback(self, future):
goal_handle = future.result()
if goal_handle.accepted:
self.result_received = True
def terminate(self, new_status):
self.node.get_logger().info(f"{self.name} terminated with status: {new_status}")
class FollowPathBehavior(Behaviour):
def __init__(self, name):
super(FollowPathBehavior, self).__init__(name)
self.node = None
self.start_time = None
self.navigation_completed = False
def setup(self, node):
self.node = node
def initialise(self):
self.start_time = time.time()
self.navigation_completed = False
self.node.get_logger().info(f"{self.name} initialized")
def update(self):
# Simulate navigation progress
elapsed = time.time() - self.start_time
if elapsed > 5.0: # Simulate completion after 5 seconds
self.navigation_completed = True
return common.Status.SUCCESS
# Simulate failure occasionally for testing
if elapsed > 2.0 and self.node.get_parameter_or('simulate_failure', False).value:
return common.Status.FAILURE
return common.Status.RUNNING
def terminate(self, new_status):
self.node.get_logger().info(f"{self.name} terminated with status: {new_status}")
class RecoveryBehavior(Behaviour):
def __init__(self, name):
super(RecoveryBehavior, self).__init__(name)
self.node = None
self.start_time = None
self.recovery_completed = False
def setup(self, node):
self.node = node
def initialise(self):
self.start_time = time.time()
self.recovery_completed = False
self.node.get_logger().info(f"{self.name} recovery started")
def update(self):
# Simulate recovery behavior (e.g., spinning in place)
elapsed = time.time() - self.start_time
if elapsed > 3.0: # Recovery takes 3 seconds
self.recovery_completed = True
return common.Status.SUCCESS
return common.Status.RUNNING
def terminate(self, new_status):
self.node.get_logger().info(f"{self.name} recovery terminated with status: {new_status}")
def main(args=None):
rclpy.init(args=args)
bt_node = Nav2BehaviorTreeNode()
try:
rclpy.spin(bt_node)
except KeyboardInterrupt:
pass
finally:
bt_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
6. Nav2 Recovery Behaviors
# Example: Nav2 Recovery Behaviors
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
import time
class Nav2RecoveryNode(Node):
def __init__(self):
super().__init__('nav2_recovery')
# Create publisher for velocity commands
self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
# Create publisher for recovery status
self.status_pub = self.create_publisher(String, 'recovery_status', 10)
# Recovery behaviors
self.recovery_behaviors = {
'spin': self.spin_recovery,
'backup': self.backup_recovery,
'wait': self.wait_recovery,
'wander': self.wander_recovery
}
# Timer for recovery execution
self.recovery_timer = self.create_timer(1.0, self.execute_recovery_cycle)
self.recovery_index = 0
self.recovery_sequence = ['spin', 'backup', 'wait', 'wander']
self.get_logger().info('Nav2 Recovery Node initialized')
def execute_recovery_cycle(self):
"""Execute a sequence of recovery behaviors"""
if len(self.recovery_sequence) == 0:
return
# Get current recovery behavior
current_behavior = self.recovery_sequence[self.recovery_index % len(self.recovery_sequence)]
# Publish status
status_msg = String()
status_msg.data = f'Executing {current_behavior} recovery'
self.status_pub.publish(status_msg)
self.get_logger().info(f'Executing {current_behavior} recovery behavior')
# Execute recovery behavior
self.recovery_behaviors[current_behavior]()
# Move to next recovery behavior
self.recovery_index += 1
def spin_recovery(self, duration=3.0):
"""Spin recovery behavior"""
self.get_logger().info(f'Executing spin recovery for {duration} seconds')
start_time = time.time()
while time.time() - start_time < duration:
twist = Twist()
twist.angular.z = 0.5 # Spin at 0.5 rad/s
self.cmd_vel_pub.publish(twist)
time.sleep(0.1)
# Stop robot
stop_twist = Twist()
self.cmd_vel_pub.publish(stop_twist)
self.get_logger().info('Spin recovery completed')
def backup_recovery(self, duration=2.0):
"""Backup recovery behavior"""
self.get_logger().info(f'Executing backup recovery for {duration} seconds')
start_time = time.time()
while time.time() - start_time < duration:
twist = Twist()
twist.linear.x = -0.2 # Move backward at 0.2 m/s
self.cmd_vel_pub.publish(twist)
time.sleep(0.1)
# Stop robot
stop_twist = Twist()
self.cmd_vel_pub.publish(stop_twist)
self.get_logger().info('Backup recovery completed')
def wait_recovery(self, duration=2.0):
"""Wait recovery behavior"""
self.get_logger().info(f'Executing wait recovery for {duration} seconds')
# Stop robot
stop_twist = Twist()
self.cmd_vel_pub.publish(stop_twist)
time.sleep(duration)
self.get_logger().info('Wait recovery completed')
def wander_recovery(self, duration=5.0):
"""Wander recovery behavior - random movement"""
self.get_logger().info(f'Executing wander recovery for {duration} seconds')
start_time = time.time()
last_command_time = start_time
while time.time() - start_time < duration:
# Change direction every second
if time.time() - last_command_time > 1.0:
twist = Twist()
# Random linear and angular velocities
import random
twist.linear.x = random.uniform(0.1, 0.3) # Forward with random speed
twist.angular.z = random.uniform(-0.5, 0.5) # Random rotation
self.cmd_vel_pub.publish(twist)
last_command_time = time.time()
time.sleep(0.1)
# Stop robot
stop_twist = Twist()
self.cmd_vel_pub.publish(stop_twist)
self.get_logger().info('Wander recovery completed')
def execute_specific_recovery(self, behavior_name, duration=None):
"""Execute a specific recovery behavior"""
if behavior_name in self.recovery_behaviors:
# Publish status
status_msg = String()
status_msg.data = f'Executing {behavior_name} recovery'
self.status_pub.publish(status_msg)
# Execute recovery behavior
if duration:
self.recovery_behaviors[behavior_name](duration)
else:
self.recovery_behaviors[behavior_name]()
# Publish completion status
status_msg.data = f'{behavior_name} recovery completed'
self.status_pub.publish(status_msg)
else:
self.get_logger().error(f'Unknown recovery behavior: {behavior_name}')
def main(args=None):
rclpy.init(args=args)
recovery_node = Nav2RecoveryNode()
try:
rclpy.spin(recovery_node)
except KeyboardInterrupt:
pass
finally:
# Stop robot before shutting down
stop_twist = Twist()
recovery_node.cmd_vel_pub.publish(stop_twist)
recovery_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
7. Complete Nav2 Launch File
# Example: Complete Nav2 Launch File
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, SetParameter
from launch_ros.substitutions import FindPackageShare
from nav2_common.launch import ReplaceString
def generate_launch_description():
# Launch Arguments
use_sim_time = LaunchConfiguration('use_sim_time', default='True')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart', default='True')
use_composition = LaunchConfiguration('use_composition', default='False')
container_name = LaunchConfiguration('container_name', default='nav2_container')
# Create the launch description and populate
ld = LaunchDescription()
# Declare launch arguments
ld.add_action(
DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use simulation (Gazebo) clock if true'
)
)
ld.add_action(
DeclareLaunchArgument(
'params_file',
default_value=PathJoinSubstitution([
FindPackageShare('nav2_bringup'),
'params',
'nav2_params.yaml'
]),
description='Full path to the ROS2 parameters file to use for all launched nodes'
)
)
ld.add_action(
DeclareLaunchArgument(
'autostart',
default_value='True',
description='Automatically startup the nav2 stack'
)
)
ld.add_action(
DeclareLaunchArgument(
'use_composition',
default_value='False',
description='Whether to launch the controller in composition mode'
)
)
ld.add_action(
DeclareLaunchArgument(
'container_name',
default_value='nav2_container',
description='the name of container that nodes will load in if use composition'
)
)
# Set parameters
ld.add_action(
SetParameter('use_sim_time', use_sim_time)
)
# Launch the AMCL node
amcl_node = Node(
package='nav2_amcl',
executable='amcl',
name='amcl',
parameters=[params_file],
remappings=[('scan', 'scan')],
condition=IfCondition(use_sim_time)
)
# Launch the map server node
map_server_node = Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
parameters=[params_file],
remappings=[('map', 'map'),
('map_metadata', 'map_metadata')]
)
# Launch the lifecycle manager
lifecycle_manager = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager_localization',
parameters=[{'use_sim_time': use_sim_time},
{'autostart': autostart},
{'node_names': ['map_server', 'amcl']}]
)
# Launch the navigation container
nav_container = Node(
condition=IfCondition(use_composition),
name=container_name,
package='rclcpp_components',
executable='component_container_isolated',
parameters=[{'autostart': autostart}]
)
# Add nodes to launch description
ld.add_action(amcl_node)
ld.add_action(map_server_node)
ld.add_action(lifecycle_manager)
ld.add_action(nav_container)
return ld
These examples provide comprehensive code examples for Nav2 navigation, covering:
- Basic navigation with action clients
- Path planning and visualization
- Robot control and movement
- Costmap processing and safety
- Behavior tree implementation
- Recovery behaviors
- Complete launch file configuration
The code follows Nav2 best practices and demonstrates how to implement navigation functionality in ROS 2.