【ROS1从入门到精通】第12篇：导航与路径规划（让机器人自主导航）
🎯 本文目标：深入学习ROS导航栈（Navigation Stack），掌握move_base框架、全局路径规划、局部路径规划、代价地图、恢复行为等核心概念，能够配置和调试完整的自主导航系统，实现机器人的智能移动。
📑 目录
1. ROS导航栈概述
1.1 什么是ROS导航栈？
ROS导航栈是一个2D导航框架，提供了从当前位置到目标位置的路径规划和执行能力。它包括：
- 定位系统：确定机器人在地图中的位置
- 感知系统：处理传感器数据，检测障碍物
- 规划系统：生成安全可行的路径
- 控制系统：执行路径，控制机器人运动
ROS Navigation Stack Costmap 2D Sensor Data Map Server Localization Odometry move_base Global Planner Local Planner Recovery Behaviors Global Path cmd_vel Robot Base
1.2 导航栈组件
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
ROS导航栈组件概览
"""
class NavigationStack:
"""导航栈组件管理"""
def __init__(self):
self.components = {
'move_base': {
'description': '导航主框架，协调所有组件',
'package': 'move_base',
'node': 'move_base_node',
'topics': {
'input': ['/map', '/scan', '/odom', '/tf'],
'output': ['/cmd_vel', '/move_base/path']
}
},
'map_server': {
'description': '提供静态地图',
'package': 'map_server',
'node': 'map_server',
'topics': {
'output': ['/map', '/map_metadata']
}
},
'amcl': {
'description': '自适应蒙特卡罗定位',
'package': 'amcl',
'node': 'amcl',
'topics': {
'input': ['/scan', '/map'],
'output': ['/amcl_pose', '/particlecloud']
}
},
'global_planner': {
'description': '全局路径规划',
'types': ['navfn', 'global_planner', 'carrot_planner'],
'interface': 'nav_core::BaseGlobalPlanner'
},
'local_planner': {
'description': '局部路径规划',
'types': ['dwa_local_planner', 'teb_local_planner', 'eband_local_planner'],
'interface': 'nav_core::BaseLocalPlanner'
},
'costmap': {
'description': '代价地图',
'types': ['static_layer', 'obstacle_layer', 'inflation_layer'],
'package': 'costmap_2d'
}
}
def get_component_info(self, component_name):
"""获取组件信息"""
return self.components.get(component_name, {})
def list_components(self):
"""列出所有组件"""
for name, info in self.components.items():
print(f"\n{name}:")
print(f" Description: {info.get('description', '')}")
if 'package' in info:
print(f" Package: {info['package']}")
if 'types' in info:
print(f" Types: {', '.join(info['types'])}")1.3 导航栈安装
bash
# 安装导航栈
sudo apt-get install ros-noetic-navigation
# 安装额外的规划器
sudo apt-get install ros-noetic-teb-local-planner
sudo apt-get install ros-noetic-dwa-local-planner
sudo apt-get install ros-noetic-global-planner
# 安装可视化工具
sudo apt-get install ros-noetic-rviz-plugin-tutorials2. move_base框架
2.1 move_base架构
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
move_base节点接口
"""
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseFeedback
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Path
from actionlib_msgs.msg import GoalStatus
class MoveBaseInterface:
"""move_base接口封装"""
def __init__(self):
rospy.init_node('move_base_interface')
# Action客户端
self.move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
# 等待move_base启动
rospy.loginfo("Waiting for move_base action server...")
self.move_base_client.wait_for_server()
rospy.loginfo("Connected to move_base server")
# 发布者
self.goal_pub = rospy.Publisher(
'/move_base_simple/goal', PoseStamped, queue_size=1)
# 订阅者
self.path_sub = rospy.Subscriber(
'/move_base/NavfnROS/plan', Path, self.path_callback)
self.cmd_vel_sub = rospy.Subscriber(
'/cmd_vel', Twist, self.cmd_vel_callback)
self.feedback_sub = rospy.Subscriber(
'/move_base/feedback', MoveBaseFeedback, self.feedback_callback)
# 状态
self.current_path = None
self.current_cmd_vel = None
self.current_feedback = None
def send_goal(self, x, y, theta):
"""发送导航目标"""
goal = MoveBaseGoal()
# 设置目标位置
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
# 设置目标姿态（四元数）
import tf.transformations as tf_trans
q = tf_trans.quaternion_from_euler(0, 0, theta)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
# 发送目标
rospy.loginfo(f"Sending goal: ({x:.2f}, {y:.2f}, {theta:.2f})")
self.move_base_client.send_goal(
goal,
done_cb=self.done_callback,
active_cb=self.active_callback,
feedback_cb=self.feedback_callback_action
)
return goal
def cancel_goal(self):
"""取消当前目标"""
self.move_base_client.cancel_goal()
rospy.loginfo("Goal cancelled")
def wait_for_result(self, timeout=None):
"""等待导航结果"""
if timeout:
result = self.move_base_client.wait_for_result(
rospy.Duration(timeout))
else:
result = self.move_base_client.wait_for_result()
return result
def get_state(self):
"""获取当前状态"""
state = self.move_base_client.get_state()
state_names = {
GoalStatus.PENDING: "PENDING",
GoalStatus.ACTIVE: "ACTIVE",
GoalStatus.PREEMPTED: "PREEMPTED",
GoalStatus.SUCCEEDED: "SUCCEEDED",
GoalStatus.ABORTED: "ABORTED",
GoalStatus.REJECTED: "REJECTED",
GoalStatus.PREEMPTING: "PREEMPTING",
GoalStatus.RECALLING: "RECALLING",
GoalStatus.RECALLED: "RECALLED",
GoalStatus.LOST: "LOST"
}
return state_names.get(state, "UNKNOWN")
def path_callback(self, msg):
"""路径回调"""
self.current_path = msg
rospy.loginfo(f"Received path with {len(msg.poses)} poses")
def cmd_vel_callback(self, msg):
"""速度命令回调"""
self.current_cmd_vel = msg
def feedback_callback(self, msg):
"""反馈回调"""
self.current_feedback = msg
def feedback_callback_action(self, feedback):
"""Action反馈回调"""
current_pose = feedback.base_position.pose
rospy.loginfo_throttle(1,
f"Current position: ({current_pose.position.x:.2f}, "
f"{current_pose.position.y:.2f})")
def active_callback(self):
"""目标激活回调"""
rospy.loginfo("Goal is now active")
def done_callback(self, state, result):
"""完成回调"""
state_name = self.get_state()
rospy.loginfo(f"Goal finished with state: {state_name}")
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal reached successfully!")
elif state == GoalStatus.ABORTED:
rospy.logwarn("Goal was aborted")
elif state == GoalStatus.PREEMPTED:
rospy.logwarn("Goal was preempted")2.2 move_base配置
yaml
# move_base_params.yaml
shutdown_costmaps: false
controller_frequency: 5.0
controller_patience: 3.0
planner_frequency: 1.0
planner_patience: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
base_global_planner: "navfn/NavfnROS"
base_local_planner: "dwa_local_planner/DWAPlannerROS"
recovery_behavior_enabled: true
clearing_rotation_allowed: true
recovery_behaviors:
- name: 'conservative_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
- name: 'rotate_recovery'
type: 'rotate_recovery/RotateRecovery'
- name: 'aggressive_reset'
type: 'clear_costmap_recovery/ClearCostmapRecovery'
conservative_reset:
reset_distance: 3.0
aggressive_reset:
reset_distance: 0.03. 代价地图（Costmap）
3.1 代价地图层
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
代价地图配置和管理
"""
import numpy as np
import rospy
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import Point
from sensor_msgs.msg import LaserScan, PointCloud2
class CostmapManager:
"""代价地图管理器"""
# 代价值定义
COST_UNKNOWN = 255 # 未知区域
COST_LETHAL = 254 # 致命障碍物
COST_INSCRIBED = 253 # 内切圆障碍
COST_FREE = 0 # 自由空间
def __init__(self):
self.layers = {
'static_layer': {
'enabled': True,
'type': 'costmap_2d::StaticLayer',
'parameters': {
'map_topic': '/map',
'subscribe_to_updates': True,
'track_unknown_space': True,
'use_maximum': False,
'lethal_cost_threshold': 100,
'unknown_cost_value': 255
}
},
'obstacle_layer': {
'enabled': True,
'type': 'costmap_2d::ObstacleLayer',
'parameters': {
'observation_sources': 'laser_scan_sensor point_cloud_sensor',
'laser_scan_sensor': {
'sensor_frame': 'laser_link',
'data_type': 'LaserScan',
'topic': '/scan',
'marking': True,
'clearing': True,
'min_obstacle_height': 0.0,
'max_obstacle_height': 2.0,
'obstacle_range': 2.5,
'raytrace_range': 3.0
},
'point_cloud_sensor': {
'sensor_frame': 'camera_link',
'data_type': 'PointCloud2',
'topic': '/points',
'marking': True,
'clearing': True,
'min_obstacle_height': 0.1,
'max_obstacle_height': 2.0,
'obstacle_range': 2.5,
'raytrace_range': 3.0
}
}
},
'inflation_layer': {
'enabled': True,
'type': 'costmap_2d::InflationLayer',
'parameters': {
'inflation_radius': 0.55,
'cost_scaling_factor': 10.0
}
},
'social_layer': {
'enabled': False,
'type': 'social_navigation_layers::ProxemicLayer',
'parameters': {
'people_topic': '/people',
'proxemic_radius': 1.2,
'cost_scaling_factor': 5.0
}
}
}
def generate_costmap_config(self, namespace='global_costmap'):
"""生成代价地图配置"""
config = {
f'{namespace}': {
'global_frame': 'map' if 'global' in namespace else 'odom',
'robot_base_frame': 'base_link',
'update_frequency': 5.0 if 'global' in namespace else 10.0,
'publish_frequency': 2.0 if 'global' in namespace else 5.0,
'static_map': True if 'global' in namespace else False,
'rolling_window': False if 'global' in namespace else True,
'width': 10.0 if 'local' in namespace else 100.0,
'height': 10.0 if 'local' in namespace else 100.0,
'resolution': 0.05,
'origin_x': 0.0,
'origin_y': 0.0,
'plugins': []
}
}
# 添加启用的层
for layer_name, layer_info in self.layers.items():
if layer_info['enabled']:
plugin = {
'name': layer_name,
'type': layer_info['type']
}
plugin.update(layer_info['parameters'])
config[namespace]['plugins'].append(plugin)
return config
def calculate_inflation_cost(self, distance, inflation_radius, cost_scaling_factor):
"""计算膨胀代价"""
if distance <= 0:
return self.COST_LETHAL
elif distance <= inflation_radius:
# 指数衰减函数
factor = np.exp(-1.0 * cost_scaling_factor *
(distance - inflation_radius))
cost = int(self.COST_INSCRIBED * factor)
return max(cost, self.COST_FREE)
else:
return self.COST_FREE
def update_costmap_params(self, namespace, params):
"""动态更新代价地图参数"""
import dynamic_reconfigure.client
client = dynamic_reconfigure.client.Client(
f"/{namespace}", timeout=5)
try:
client.update_configuration(params)
rospy.loginfo(f"Updated {namespace} parameters")
except Exception as e:
rospy.logerr(f"Failed to update parameters: {e}")
def visualize_costmap(self, costmap_msg):
"""可视化代价地图"""
import matplotlib.pyplot as plt
import matplotlib.colors as colors
# 转换为numpy数组
width = costmap_msg.info.width
height = costmap_msg.info.height
data = np.array(costmap_msg.data).reshape((height, width))
# 创建自定义颜色映射
cmap = colors.ListedColormap([
'white', # FREE (0)
'yellow', # LOW COST
'orange', # MEDIUM COST
'red', # HIGH COST
'black', # LETHAL (254)
'gray' # UNKNOWN (255)
])
# 设置边界
bounds = [0, 50, 100, 200, 254, 255, 256]
norm = colors.BoundaryNorm(bounds, cmap.N)
# 绘制
plt.figure(figsize=(10, 10))
plt.imshow(data, cmap=cmap, norm=norm, origin='lower')
plt.colorbar(label='Cost Value')
plt.title('Costmap Visualization')
plt.xlabel('X (cells)')
plt.ylabel('Y (cells)')
plt.show()3.2 代价地图配置文件
yaml
# costmap_common_params.yaml
footprint: [[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]]
footprint_padding: 0.01
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
resolution: 0.05
obstacle_range: 2.5
raytrace_range: 3.0
# 障碍物层参数
obstacle_layer:
enabled: true
obstacle_range: 2.5
raytrace_range: 3.0
inflation_radius: 0.2
track_unknown_space: true
combination_method: 1
observation_sources: laser_scan_sensor
laser_scan_sensor:
sensor_frame: laser_link
data_type: LaserScan
topic: /scan
marking: true
clearing: true
min_obstacle_height: 0.0
max_obstacle_height: 2.0
# 膨胀层参数
inflation_layer:
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 10.0
# 静态层参数
static_layer:
enabled: true
map_topic: /map4. 全局路径规划
4.1 全局规划器实现
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
全局路径规划器
"""
import rospy
import numpy as np
from nav_msgs.msg import Path, OccupancyGrid
from geometry_msgs.msg import PoseStamped
import heapq
class GlobalPlanner:
"""全局路径规划器基类"""
def __init__(self):
self.costmap = None
self.resolution = 0.05
self.origin_x = 0
self.origin_y = 0
self.width = 0
self.height = 0
def set_costmap(self, costmap_msg):
"""设置代价地图"""
self.costmap = np.array(costmap_msg.data).reshape(
(costmap_msg.info.height, costmap_msg.info.width))
self.resolution = costmap_msg.info.resolution
self.origin_x = costmap_msg.info.origin.position.x
self.origin_y = costmap_msg.info.origin.position.y
self.width = costmap_msg.info.width
self.height = costmap_msg.info.height
def world_to_map(self, wx, wy):
"""世界坐标转地图坐标"""
mx = int((wx - self.origin_x) / self.resolution)
my = int((wy - self.origin_y) / self.resolution)
return mx, my
def map_to_world(self, mx, my):
"""地图坐标转世界坐标"""
wx = self.origin_x + (mx + 0.5) * self.resolution
wy = self.origin_y + (my + 0.5) * self.resolution
return wx, wy
def plan(self, start, goal):
"""规划路径（需要子类实现）"""
raise NotImplementedError
class AStarPlanner(GlobalPlanner):
"""A*路径规划器"""
def __init__(self):
super().__init__()
self.cost_threshold = 252 # 致命障碍物阈值
def heuristic(self, a, b):
"""启发式函数（欧几里得距离）"""
return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def get_neighbors(self, node):
"""获取邻居节点"""
neighbors = []
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1),
(-1,-1), (-1,1), (1,-1), (1,1)]:
x, y = node[0] + dx, node[1] + dy
# 检查边界
if 0 <= x < self.width and 0 <= y < self.height:
# 检查是否可通行
if self.costmap[y, x] < self.cost_threshold:
# 对角移动的代价更高
if dx != 0 and dy != 0:
cost = np.sqrt(2) * self.resolution
else:
cost = self.resolution
# 加上地图代价
cost += self.costmap[y, x] / 255.0
neighbors.append(((x, y), cost))
return neighbors
def plan(self, start, goal):
"""A*路径规划"""
if self.costmap is None:
rospy.logwarn("No costmap available")
return None
# 转换为地图坐标
start_map = self.world_to_map(start[0], start[1])
goal_map = self.world_to_map(goal[0], goal[1])
# 检查起点和终点是否有效
if not (0 <= start_map[0] < self.width and
0 <= start_map[1] < self.height):
rospy.logwarn("Start position out of bounds")
return None
if not (0 <= goal_map[0] < self.width and
0 <= goal_map[1] < self.height):
rospy.logwarn("Goal position out of bounds")
return None
if self.costmap[start_map[1], start_map[0]] >= self.cost_threshold:
rospy.logwarn("Start position is in obstacle")
return None
if self.costmap[goal_map[1], goal_map[0]] >= self.cost_threshold:
rospy.logwarn("Goal position is in obstacle")
return None
# A*算法
open_set = [(0, start_map)]
came_from = {}
g_score = {start_map: 0}
f_score = {start_map: self.heuristic(start_map, goal_map)}
while open_set:
current = heapq.heappop(open_set)[1]
# 到达目标
if current == goal_map:
# 重建路径
path = []
while current in came_from:
wx, wy = self.map_to_world(current[0], current[1])
path.append((wx, wy))
current = came_from[current]
wx, wy = self.map_to_world(start_map[0], start_map[1])
path.append((wx, wy))
path.reverse()
return path
# 扩展邻居
for neighbor, move_cost in self.get_neighbors(current):
tentative_g_score = g_score[current] + move_cost
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = tentative_g_score + self.heuristic(neighbor, goal_map)
heapq.heappush(open_set, (f_score[neighbor], neighbor))
rospy.logwarn("No path found")
return None
class DijkstraPlanner(GlobalPlanner):
"""Dijkstra路径规划器"""
def __init__(self):
super().__init__()
self.cost_threshold = 252
def plan(self, start, goal):
"""Dijkstra路径规划"""
if self.costmap is None:
return None
# 转换为地图坐标
start_map = self.world_to_map(start[0], start[1])
goal_map = self.world_to_map(goal[0], goal[1])
# 初始化
distances = np.full((self.height, self.width), np.inf)
distances[start_map[1], start_map[0]] = 0
visited = np.zeros((self.height, self.width), dtype=bool)
parent = {}
# 优先队列
pq = [(0, start_map)]
while pq:
current_dist, current = heapq.heappop(pq)
if visited[current[1], current[0]]:
continue
visited[current[1], current[0]] = True
# 到达目标
if current == goal_map:
# 重建路径
path = []
while current in parent:
wx, wy = self.map_to_world(current[0], current[1])
path.append((wx, wy))
current = parent[current]
wx, wy = self.map_to_world(start_map[0], start_map[1])
path.append((wx, wy))
path.reverse()
return path
# 检查邻居
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
nx, ny = current[0] + dx, current[1] + dy
if (0 <= nx < self.width and 0 <= ny < self.height and
not visited[ny, nx] and
self.costmap[ny, nx] < self.cost_threshold):
new_dist = current_dist + 1 + self.costmap[ny, nx] / 255.0
if new_dist < distances[ny, nx]:
distances[ny, nx] = new_dist
parent[(nx, ny)] = current
heapq.heappush(pq, (new_dist, (nx, ny)))
return None4.2 全局规划器配置
yaml
# global_planner_params.yaml
# NavfnROS参数
NavfnROS:
visualize_potential: false
allow_unknown: true
planner_window_x: 0.0
planner_window_y: 0.0
default_tolerance: 0.0
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false
# GlobalPlanner参数
GlobalPlanner:
old_navfn_behavior: false
use_quadratic: true
use_dijkstra: true
use_grid_path: false
allow_unknown: true
planner_window_x: 0.0
planner_window_y: 0.0
default_tolerance: 0.0
publish_scale: 100
planner_costmap_publish_frequency: 0.0
lethal_cost: 253
neutral_cost: 50
cost_factor: 3.0
publish_potential: true
orientation_mode: 0
orientation_window_size: 15. 局部路径规划
5.1 DWA局部规划器
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
DWA（动态窗口法）局部规划器
"""
import rospy
import numpy as np
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry, Path
from sensor_msgs.msg import LaserScan
class DWAPlanner:
"""DWA局部路径规划器"""
def __init__(self):
# 机器人参数
self.max_vel_x = 0.5 # 最大线速度 m/s
self.min_vel_x = -0.1 # 最小线速度 m/s
self.max_vel_theta = 1.0 # 最大角速度 rad/s
self.min_vel_theta = -1.0 # 最小角速度 rad/s
self.acc_lim_x = 0.5 # 线加速度限制 m/s^2
self.acc_lim_theta = 1.0 # 角加速度限制 rad/s^2
# DWA参数
self.dt = 0.1 # 时间步长
self.predict_time = 3.0 # 预测时间
self.velocity_samples_x = 20 # 线速度采样数
self.velocity_samples_theta = 40 # 角速度采样数
# 评价函数权重
self.path_distance_bias = 32.0 # 路径距离权重
self.goal_distance_bias = 24.0 # 目标距离权重
self.occdist_scale = 0.01 # 障碍物距离权重
# 当前状态
self.current_pose = None
self.current_velocity = [0, 0] # [v, w]
self.laser_scan = None
self.global_path = None
def dynamic_window(self, current_vel):
"""计算动态窗口"""
# 速度限制窗口
vs = [self.min_vel_x, self.max_vel_x,
self.min_vel_theta, self.max_vel_theta]
# 加速度限制窗口
vd = [current_vel[0] - self.acc_lim_x * self.dt,
current_vel[0] + self.acc_lim_x * self.dt,
current_vel[1] - self.acc_lim_theta * self.dt,
current_vel[1] + self.acc_lim_theta * self.dt]
# 动态窗口 = 速度限制 ∩ 加速度限制
dw = [max(vs[0], vd[0]), min(vs[1], vd[1]),
max(vs[2], vd[2]), min(vs[3], vd[3])]
return dw
def predict_trajectory(self, init_pose, velocity, predict_time):
"""预测轨迹"""
trajectory = []
pose = init_pose.copy()
time_steps = int(predict_time / self.dt)
for _ in range(time_steps):
# 更新位置
pose[0] += velocity[0] * np.cos(pose[2]) * self.dt
pose[1] += velocity[0] * np.sin(pose[2]) * self.dt
pose[2] += velocity[1] * self.dt
trajectory.append(pose.copy())
return np.array(trajectory)
def calc_obstacle_cost(self, trajectory, obstacles):
"""计算障碍物代价"""
if obstacles is None or len(obstacles) == 0:
return 0
min_dist = float('inf')
for pose in trajectory:
for obs in obstacles:
dist = np.sqrt((pose[0] - obs[0])**2 +
(pose[1] - obs[1])**2)
if dist < min_dist:
min_dist = dist
# 碰撞检测
if min_dist < 0.2: # 机器人半径
return float('inf')
return 1.0 / min_dist
def calc_goal_cost(self, trajectory, goal):
"""计算目标代价"""
final_pose = trajectory[-1]
goal_dist = np.sqrt((final_pose[0] - goal[0])**2 +
(final_pose[1] - goal[1])**2)
return goal_dist
def calc_path_cost(self, trajectory, path):
"""计算路径跟踪代价"""
if path is None or len(path) == 0:
return 0
min_dist = float('inf')
for pose in trajectory:
for path_point in path:
dist = np.sqrt((pose[0] - path_point[0])**2 +
(pose[1] - path_point[1])**2)
if dist < min_dist:
min_dist = dist
return min_dist
def evaluate_trajectory(self, trajectory, goal, obstacles, path):
"""评价轨迹"""
# 计算各项代价
goal_cost = self.calc_goal_cost(trajectory, goal)
obs_cost = self.calc_obstacle_cost(trajectory, obstacles)
path_cost = self.calc_path_cost(trajectory, path)
# 如果会碰撞，返回无穷大代价
if obs_cost == float('inf'):
return float('inf')
# 总代价
total_cost = (self.goal_distance_bias * goal_cost +
self.occdist_scale * obs_cost +
self.path_distance_bias * path_cost)
return total_cost
def search_best_trajectory(self, current_pose, current_vel, goal, obstacles, path):
"""搜索最优轨迹"""
# 计算动态窗口
dw = self.dynamic_window(current_vel)
best_trajectory = None
min_cost = float('inf')
best_velocity = [0, 0]
# 速度采样
for vx in np.linspace(dw[0], dw[1], self.velocity_samples_x):
for vtheta in np.linspace(dw[2], dw[3], self.velocity_samples_theta):
# 预测轨迹
velocity = [vx, vtheta]
trajectory = self.predict_trajectory(
current_pose, velocity, self.predict_time)
# 评价轨迹
cost = self.evaluate_trajectory(
trajectory, goal, obstacles, path)
# 更新最优
if cost < min_cost:
min_cost = cost
best_trajectory = trajectory
best_velocity = velocity
return best_velocity, best_trajectory
def control(self, goal):
"""DWA控制"""
if self.current_pose is None:
return Twist()
# 从激光数据提取障碍物
obstacles = self.extract_obstacles()
# 搜索最优速度
best_velocity, best_trajectory = self.search_best_trajectory(
self.current_pose,
self.current_velocity,
goal,
obstacles,
self.global_path
)
# 生成速度命令
cmd_vel = Twist()
cmd_vel.linear.x = best_velocity[0]
cmd_vel.angular.z = best_velocity[1]
# 更新当前速度
self.current_velocity = best_velocity
return cmd_vel
def extract_obstacles(self):
"""从激光数据提取障碍物"""
if self.laser_scan is None:
return []
obstacles = []
for i, range_val in enumerate(self.laser_scan.ranges):
if range_val < self.laser_scan.range_max:
angle = self.laser_scan.angle_min + i * self.laser_scan.angle_increment
# 转换到机器人坐标系
obs_x = range_val * np.cos(angle)
obs_y = range_val * np.sin(angle)
# 转换到世界坐标系
world_x = self.current_pose[0] + \
obs_x * np.cos(self.current_pose[2]) - \
obs_y * np.sin(self.current_pose[2])
world_y = self.current_pose[1] + \
obs_x * np.sin(self.current_pose[2]) + \
obs_y * np.cos(self.current_pose[2])
obstacles.append([world_x, world_y])
return obstacles5.2 局部规划器配置
yaml
# dwa_local_planner_params.yaml
DWAPlannerROS:
# 机器人配置参数
max_vel_x: 0.5
min_vel_x: 0.0
max_vel_y: 0.0
min_vel_y: 0.0
max_vel_trans: 0.5
min_vel_trans: 0.1
max_vel_theta: 1.0
min_vel_theta: 0.2
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
acc_lim_trans: 2.5
# 目标容差
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
latch_xy_goal_tolerance: false
# 前向仿真参数
sim_time: 1.7
sim_granularity: 0.025
vx_samples: 20
vy_samples: 0
vtheta_samples: 40
# 轨迹评分参数
path_distance_bias: 32.0
goal_distance_bias: 24.0
occdist_scale: 0.01
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# 振荡预防
oscillation_reset_dist: 0.05
# 调试
publish_traj_pc: true
publish_cost_grid_pc: true6. 恢复行为
6.1 恢复行为实现
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
导航恢复行为
"""
import rospy
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
import tf2_ros
import math
class RecoveryBehaviors:
"""恢复行为管理"""
def __init__(self):
rospy.init_node('recovery_behaviors')
# 速度发布
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# 清除代价地图服务
self.clear_costmap_srv = rospy.ServiceProxy(
'/move_base/clear_costmaps', Empty)
# TF
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# 恢复行为列表
self.behaviors = {
'rotate_recovery': self.rotate_recovery,
'clear_costmap': self.clear_costmap_recovery,
'backup_recovery': self.backup_recovery,
'aggressive_reset': self.aggressive_reset
}
def rotate_recovery(self, angle=2*math.pi, speed=0.5):
"""旋转恢复"""
rospy.loginfo("Executing rotate recovery...")
# 计算旋转时间
duration = abs(angle / speed)
# 发送旋转命令
cmd = Twist()
cmd.angular.z = speed if angle > 0 else -speed
start_time = rospy.Time.now()
rate = rospy.Rate(10)
while (rospy.Time.now() - start_time).to_sec() < duration:
self.cmd_vel_pub.publish(cmd)
rate.sleep()
# 停止
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("Rotate recovery completed")
return True
def clear_costmap_recovery(self):
"""清除代价地图恢复"""
rospy.loginfo("Executing clear costmap recovery...")
try:
self.clear_costmap_srv()
rospy.loginfo("Costmaps cleared successfully")
return True
except rospy.ServiceException as e:
rospy.logerr(f"Failed to clear costmaps: {e}")
return False
def backup_recovery(self, distance=0.3, speed=0.1):
"""后退恢复"""
rospy.loginfo("Executing backup recovery...")
# 计算后退时间
duration = abs(distance / speed)
# 发送后退命令
cmd = Twist()
cmd.linear.x = -abs(speed)
start_time = rospy.Time.now()
rate = rospy.Rate(10)
while (rospy.Time.now() - start_time).to_sec() < duration:
self.cmd_vel_pub.publish(cmd)
rate.sleep()
# 停止
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("Backup recovery completed")
return True
def aggressive_reset(self):
"""激进重置"""
rospy.loginfo("Executing aggressive reset...")
# 1. 停止机器人
self.cmd_vel_pub.publish(Twist())
rospy.sleep(0.5)
# 2. 清除代价地图
self.clear_costmap_recovery()
# 3. 小幅度旋转
self.rotate_recovery(angle=0.5, speed=0.3)
# 4. 再次清除代价地图
self.clear_costmap_recovery()
rospy.loginfo("Aggressive reset completed")
return True
def execute_behavior_sequence(self, sequence):
"""执行恢复行为序列"""
rospy.loginfo(f"Executing recovery sequence: {sequence}")
for behavior_name in sequence:
if behavior_name in self.behaviors:
success = self.behaviors[behavior_name]()
if not success:
rospy.logwarn(f"Recovery behavior '{behavior_name}' failed")
return False
# 行为间延迟
rospy.sleep(0.5)
else:
rospy.logwarn(f"Unknown recovery behavior: {behavior_name}")
return True
def monitor_and_recover(self):
"""监控并执行恢复"""
# 标准恢复序列
recovery_sequences = [
['clear_costmap'],
['rotate_recovery', 'clear_costmap'],
['backup_recovery', 'rotate_recovery', 'clear_costmap'],
['aggressive_reset']
]
recovery_level = 0
while not rospy.is_shutdown():
# 检查是否需要恢复（这里需要实现具体的检测逻辑）
if self.need_recovery():
if recovery_level < len(recovery_sequences):
sequence = recovery_sequences[recovery_level]
success = self.execute_behavior_sequence(sequence)
if success:
recovery_level = 0 # 重置恢复级别
else:
recovery_level += 1 # 升级恢复级别
else:
rospy.logerr("All recovery behaviors failed!")
recovery_level = 0
rospy.sleep(1.0)
def need_recovery(self):
"""检查是否需要恢复"""
# 这里应该实现具体的检测逻辑
# 例如：检查机器人是否卡住、路径是否被阻塞等
return False7. 导航配置与调优
7.1 导航参数调优
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
导航参数自动调优
"""
import rospy
import dynamic_reconfigure.client
import numpy as np
from move_base_msgs.msg import MoveBaseActionResult
from geometry_msgs.msg import PoseWithCovarianceStamped
class NavigationTuner:
"""导航参数调优器"""
def __init__(self):
# 参数范围
self.param_ranges = {
'max_vel_x': (0.1, 1.0),
'max_vel_theta': (0.2, 2.0),
'acc_lim_x': (0.5, 3.0),
'acc_lim_theta': (0.5, 4.0),
'xy_goal_tolerance': (0.05, 0.5),
'yaw_goal_tolerance': (0.05, 0.5),
'path_distance_bias': (10.0, 100.0),
'goal_distance_bias': (10.0, 100.0),
'occdist_scale': (0.001, 0.1),
'inflation_radius': (0.3, 1.0),
'cost_scaling_factor': (1.0, 20.0)
}
# 性能指标
self.metrics = {
'success_rate': 0,
'avg_time': 0,
'avg_path_length': 0,
'collision_count': 0,
'oscillation_count': 0
}
# 动态配置客户端
self.dwa_client = dynamic_reconfigure.client.Client(
"/move_base/DWAPlannerROS")
self.costmap_client = dynamic_reconfigure.client.Client(
"/move_base/global_costmap/inflation_layer")
def evaluate_performance(self, test_goals):
"""评估导航性能"""
results = []
for goal in test_goals:
start_time = rospy.Time.now()
# 发送目标
success = self.send_goal_and_wait(goal)
end_time = rospy.Time.now()
duration = (end_time - start_time).to_sec()
results.append({
'success': success,
'time': duration,
'goal': goal
})
# 计算指标
success_count = sum(1 for r in results if r['success'])
self.metrics['success_rate'] = success_count / len(results)
if success_count > 0:
successful_results = [r for r in results if r['success']]
self.metrics['avg_time'] = np.mean([r['time'] for r in successful_results])
return self.metrics
def grid_search(self, param_names, test_goals, resolution=3):
"""网格搜索最优参数"""
best_params = {}
best_score = -float('inf')
# 生成参数网格
param_grid = self.generate_param_grid(param_names, resolution)
for params in param_grid:
# 应用参数
self.apply_params(params)
# 评估性能
metrics = self.evaluate_performance(test_goals)
# 计算得分
score = self.calculate_score(metrics)
# 更新最优
if score > best_score:
best_score = score
best_params = params.copy()
rospy.loginfo(f"Params: {params}, Score: {score}")
return best_params, best_score
def generate_param_grid(self, param_names, resolution):
"""生成参数网格"""
grid = []
# 简化：只生成每个参数的几个值
for param_name in param_names:
if param_name in self.param_ranges:
min_val, max_val = self.param_ranges[param_name]
values = np.linspace(min_val, max_val, resolution)
for value in values:
grid.append({param_name: value})
return grid
def calculate_score(self, metrics):
"""计算性能得分"""
# 加权组合多个指标
score = (metrics['success_rate'] * 100 -
metrics['avg_time'] * 0.1 -
metrics['collision_count'] * 10 -
metrics['oscillation_count'] * 5)
return score
def apply_params(self, params):
"""应用参数"""
try:
# 分离DWA参数和代价地图参数
dwa_params = {}
costmap_params = {}
for key, value in params.items():
if key in ['inflation_radius', 'cost_scaling_factor']:
costmap_params[key] = value
else:
dwa_params[key] = value
# 应用参数
if dwa_params:
self.dwa_client.update_configuration(dwa_params)
if costmap_params:
self.costmap_client.update_configuration(costmap_params)
rospy.sleep(0.5) # 等待参数生效
except Exception as e:
rospy.logerr(f"Failed to apply params: {e}")
def adaptive_tuning(self):
"""自适应参数调整"""
# 监控导航性能
performance_window = []
window_size = 10
while not rospy.is_shutdown():
# 获取最新性能数据
current_performance = self.get_current_performance()
performance_window.append(current_performance)
if len(performance_window) > window_size:
performance_window.pop(0)
# 分析性能趋势
if len(performance_window) >= window_size:
trend = self.analyze_trend(performance_window)
# 根据趋势调整参数
if trend == 'too_slow':
self.increase_speed()
elif trend == 'too_aggressive':
self.decrease_speed()
elif trend == 'oscillating':
self.reduce_oscillation()
rospy.sleep(1.0)
def increase_speed(self):
"""提高速度"""
current = self.dwa_client.get_configuration()
new_params = {
'max_vel_x': min(current['max_vel_x'] * 1.1,
self.param_ranges['max_vel_x'][1]),
'max_vel_theta': min(current['max_vel_theta'] * 1.1,
self.param_ranges['max_vel_theta'][1])
}
self.dwa_client.update_configuration(new_params)
rospy.loginfo("Increased speed parameters")
def decrease_speed(self):
"""降低速度"""
current = self.dwa_client.get_configuration()
new_params = {
'max_vel_x': max(current['max_vel_x'] * 0.9,
self.param_ranges['max_vel_x'][0]),
'max_vel_theta': max(current['max_vel_theta'] * 0.9,
self.param_ranges['max_vel_theta'][0])
}
self.dwa_client.update_configuration(new_params)
rospy.loginfo("Decreased speed parameters")
def reduce_oscillation(self):
"""减少振荡"""
current = self.dwa_client.get_configuration()
new_params = {
'oscillation_reset_dist': current.get('oscillation_reset_dist', 0.05) * 1.2,
'path_distance_bias': current.get('path_distance_bias', 32.0) * 1.1
}
self.dwa_client.update_configuration(new_params)
rospy.loginfo("Adjusted oscillation parameters")8. 导航目标发送
8.1 多目标导航
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
多目标导航管理
"""
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker, MarkerArray
import yaml
class MultiGoalNavigation:
"""多目标导航管理器"""
def __init__(self):
rospy.init_node('multi_goal_navigation')
# Action客户端
self.move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
self.move_base_client.wait_for_server()
# 目标点管理
self.goals = []
self.current_goal_index = 0
self.patrol_mode = False
# 可视化
self.marker_pub = rospy.Publisher(
'/navigation_goals', MarkerArray, queue_size=1)
# 目标订阅
self.goal_sub = rospy.Subscriber(
'/clicked_point', PointStamped, self.add_goal_callback)
# 加载预设目标点
self.load_goals_from_file()
def load_goals_from_file(self, filename='goals.yaml'):
"""从文件加载目标点"""
try:
with open(filename, 'r') as f:
data = yaml.safe_load(f)
self.goals = []
for goal_data in data['goals']:
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.pose.position.x = goal_data['x']
goal.pose.position.y = goal_data['y']
goal.pose.position.z = 0
# 四元数
goal.pose.orientation.x = goal_data.get('qx', 0)
goal.pose.orientation.y = goal_data.get('qy', 0)
goal.pose.orientation.z = goal_data.get('qz', 0)
goal.pose.orientation.w = goal_data.get('qw', 1)
self.goals.append(goal)
rospy.loginfo(f"Loaded {len(self.goals)} goals from file")
except Exception as e:
rospy.logwarn(f"Failed to load goals: {e}")
def save_goals_to_file(self, filename='goals.yaml'):
"""保存目标点到文件"""
data = {'goals': []}
for goal in self.goals:
goal_data = {
'x': goal.pose.position.x,
'y': goal.pose.position.y,
'qx': goal.pose.orientation.x,
'qy': goal.pose.orientation.y,
'qz': goal.pose.orientation.z,
'qw': goal.pose.orientation.w
}
data['goals'].append(goal_data)
try:
with open(filename, 'w') as f:
yaml.dump(data, f)
rospy.loginfo(f"Saved {len(self.goals)} goals to file")
except Exception as e:
rospy.logerr(f"Failed to save goals: {e}")
def add_goal(self, pose_stamped):
"""添加目标点"""
self.goals.append(pose_stamped)
self.visualize_goals()
rospy.loginfo(f"Added goal #{len(self.goals)}")
def remove_goal(self, index):
"""移除目标点"""
if 0 <= index < len(self.goals):
self.goals.pop(index)
self.visualize_goals()
rospy.loginfo(f"Removed goal #{index}")
def clear_goals(self):
"""清除所有目标点"""
self.goals = []
self.current_goal_index = 0
self.visualize_goals()
rospy.loginfo("Cleared all goals")
def start_navigation(self):
"""开始导航"""
if not self.goals:
rospy.logwarn("No goals to navigate")
return
self.current_goal_index = 0
self.send_next_goal()
def send_next_goal(self):
"""发送下一个目标"""
if self.current_goal_index >= len(self.goals):
if self.patrol_mode:
self.current_goal_index = 0
rospy.loginfo("Restarting patrol from first goal")
else:
rospy.loginfo("All goals completed")
return
goal = self.goals[self.current_goal_index]
# 转换为MoveBaseGoal
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = goal
move_base_goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo(f"Sending goal #{self.current_goal_index + 1}/{len(self.goals)}")
self.move_base_client.send_goal(
move_base_goal,
done_cb=self.goal_done_callback
)
def goal_done_callback(self, state, result):
"""目标完成回调"""
if state == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo(f"Goal #{self.current_goal_index + 1} reached successfully")
self.current_goal_index += 1
self.send_next_goal()
else:
rospy.logwarn(f"Failed to reach goal #{self.current_goal_index + 1}")
# 可选：重试或跳过
retry = rospy.get_param('~retry_failed_goals', False)
if not retry:
self.current_goal_index += 1
self.send_next_goal()
def visualize_goals(self):
"""可视化目标点"""
marker_array = MarkerArray()
# 清除旧标记
clear_marker = Marker()
clear_marker.header.frame_id = 'map'
clear_marker.action = Marker.DELETEALL
marker_array.markers.append(clear_marker)
# 添加目标标记
for i, goal in enumerate(self.goals):
# 目标点标记
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time.now()
marker.ns = 'goals'
marker.id = i
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = goal.pose
marker.scale.x = 0.5
marker.scale.y = 0.1
marker.scale.z = 0.1
# 当前目标用不同颜色
if i == self.current_goal_index:
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
else:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker_array.markers.append(marker)
# 文字标记
text_marker = Marker()
text_marker.header = marker.header
text_marker.ns = 'goal_texts'
text_marker.id = i + 1000
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose = goal.pose
text_marker.pose.position.z += 0.5
text_marker.text = f"Goal {i+1}"
text_marker.scale.z = 0.3
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
# 连接线
if len(self.goals) > 1:
line_marker = Marker()
line_marker.header.frame_id = 'map'
line_marker.header.stamp = rospy.Time.now()
line_marker.ns = 'path'
line_marker.id = 2000
line_marker.type = Marker.LINE_STRIP
line_marker.action = Marker.ADD
for goal in self.goals:
line_marker.points.append(goal.pose.position)
if self.patrol_mode:
# 闭合路径
line_marker.points.append(self.goals[0].pose.position)
line_marker.scale.x = 0.05
line_marker.color.r = 0.0
line_marker.color.g = 0.0
line_marker.color.b = 1.0
line_marker.color.a = 0.5
marker_array.markers.append(line_marker)
self.marker_pub.publish(marker_array)
def set_patrol_mode(self, enabled):
"""设置巡逻模式"""
self.patrol_mode = enabled
mode = "enabled" if enabled else "disabled"
rospy.loginfo(f"Patrol mode {mode}")
self.visualize_goals()9. 多机器人导航
9.1 多机器人协调
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
多机器人导航协调
"""
import rospy
from geometry_msgs.msg import PoseStamped, Twist
from nav_msgs.msg import Path
import threading
class MultiRobotCoordinator:
"""多机器人协调器"""
def __init__(self, robot_names):
rospy.init_node('multi_robot_coordinator')
self.robots = {}
# 为每个机器人创建接口
for name in robot_names:
self.robots[name] = {
'namespace': name,
'pose': None,
'path': None,
'goal': None,
'status': 'idle',
'priority': 0
}
# 订阅位姿
pose_sub = rospy.Subscriber(
f'/{name}/amcl_pose',
PoseWithCovarianceStamped,
lambda msg, n=name: self.pose_callback(msg, n)
)
# 订阅路径
path_sub = rospy.Subscriber(
f'/{name}/move_base/NavfnROS/plan',
Path,
lambda msg, n=name: self.path_callback(msg, n)
)
# 目标发布
self.robots[name]['goal_pub'] = rospy.Publisher(
f'/{name}/move_base_simple/goal',
PoseStamped,
queue_size=1
)
# 速度控制
self.robots[name]['cmd_vel_pub'] = rospy.Publisher(
f'/{name}/cmd_vel',
Twist,
queue_size=1
)
# 冲突检测线程
self.conflict_thread = threading.Thread(target=self.conflict_detection_loop)
self.conflict_thread.start()
def pose_callback(self, msg, robot_name):
"""位姿回调"""
self.robots[robot_name]['pose'] = msg.pose.pose
def path_callback(self, msg, robot_name):
"""路径回调"""
self.robots[robot_name]['path'] = msg
def send_goal(self, robot_name, goal):
"""发送目标"""
if robot_name in self.robots:
self.robots[robot_name]['goal'] = goal
self.robots[robot_name]['goal_pub'].publish(goal)
self.robots[robot_name]['status'] = 'navigating'
rospy.loginfo(f"Sent goal to {robot_name}")
def stop_robot(self, robot_name):
"""停止机器人"""
if robot_name in self.robots:
stop_cmd = Twist()
self.robots[robot_name]['cmd_vel_pub'].publish(stop_cmd)
self.robots[robot_name]['status'] = 'stopped'
rospy.loginfo(f"Stopped {robot_name}")
def conflict_detection_loop(self):
"""冲突检测循环"""
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.detect_and_resolve_conflicts()
rate.sleep()
def detect_and_resolve_conflicts(self):
"""检测并解决冲突"""
robot_names = list(self.robots.keys())
for i in range(len(robot_names)):
for j in range(i + 1, len(robot_names)):
robot1 = robot_names[i]
robot2 = robot_names[j]
# 检查路径冲突
if self.check_path_conflict(robot1, robot2):
self.resolve_conflict(robot1, robot2)
def check_path_conflict(self, robot1, robot2):
"""检查路径冲突"""
if (self.robots[robot1]['path'] is None or
self.robots[robot2]['path'] is None):
return False
path1 = self.robots[robot1]['path'].poses
path2 = self.robots[robot2]['path'].poses
# 检查路径交叉
min_distance = 0.5 # 最小安全距离
for pose1 in path1[:20]: # 只检查近期路径
for pose2 in path2[:20]:
dx = pose1.pose.position.x - pose2.pose.position.x
dy = pose1.pose.position.y - pose2.pose.position.y
distance = (dx**2 + dy**2)**0.5
if distance < min_distance:
return True
return False
def resolve_conflict(self, robot1, robot2):
"""解决冲突"""
rospy.logwarn(f"Conflict detected between {robot1} and {robot2}")
# 基于优先级解决
if self.robots[robot1]['priority'] > self.robots[robot2]['priority']:
# robot1优先，robot2等待
self.stop_robot(robot2)
rospy.loginfo(f"{robot2} yielding to {robot1}")
elif self.robots[robot2]['priority'] > self.robots[robot1]['priority']:
# robot2优先，robot1等待
self.stop_robot(robot1)
rospy.loginfo(f"{robot1} yielding to {robot2}")
else:
# 相同优先级，基于距离决定
if self.get_goal_distance(robot1) < self.get_goal_distance(robot2):
self.stop_robot(robot2)
else:
self.stop_robot(robot1)
def get_goal_distance(self, robot_name):
"""获取到目标的距离"""
robot = self.robots[robot_name]
if robot['pose'] is None or robot['goal'] is None:
return float('inf')
dx = robot['goal'].pose.position.x - robot['pose'].position.x
dy = robot['goal'].pose.position.y - robot['pose'].position.y
return (dx**2 + dy**2)**0.5
def coordinate_formation(self, formation_type='line'):
"""编队控制"""
robot_names = list(self.robots.keys())
if formation_type == 'line':
# 直线编队
spacing = 1.0
for i, name in enumerate(robot_names):
offset_x = i * spacing
offset_y = 0
# 计算编队位置...
elif formation_type == 'triangle':
# 三角编队
pass
elif formation_type == 'square':
# 方形编队
pass10. 实战案例：完整导航系统
10.1 完整导航系统实现
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
完整的自主导航系统
"""
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav_msgs.msg import OccupancyGrid, Path
from sensor_msgs.msg import LaserScan
from std_srvs.srv import Empty
import tf2_ros
import numpy as np
class AutonomousNavigationSystem:
"""自主导航系统"""
def __init__(self):
rospy.init_node('autonomous_navigation_system')
# 系统组件
self.components = {
'localization': False,
'map': False,
'move_base': False,
'sensors': False
}
# 初始化组件
self.setup_localization()
self.setup_navigation()
self.setup_sensors()
self.setup_recovery()
# 状态机
self.state = 'idle'
self.state_machine = {
'idle': self.idle_state,
'localizing': self.localizing_state,
'planning': self.planning_state,
'navigating': self.navigating_state,
'recovering': self.recovering_state,
'reached': self.reached_state
}
# 主循环
self.main_loop()
def setup_localization(self):
"""设置定位"""
# 初始位姿发布
self.initial_pose_pub = rospy.Publisher(
'/initialpose', PoseWithCovarianceStamped, queue_size=1)
# AMCL位姿订阅
self.amcl_pose_sub = rospy.Subscriber(
'/amcl_pose', PoseWithCovarianceStamped,
self.amcl_pose_callback)
# TF
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.current_pose = None
self.localization_confidence = 0
def setup_navigation(self):
"""设置导航"""
# Move base客户端
self.move_base_client = actionlib.SimpleActionClient(
'move_base', MoveBaseAction)
rospy.loginfo("Waiting for move_base...")
if self.move_base_client.wait_for_server(rospy.Duration(5)):
self.components['move_base'] = True
rospy.loginfo("Connected to move_base")
else:
rospy.logwarn("move_base not available")
# 地图订阅
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
# 路径订阅
self.global_path_sub = rospy.Subscriber(
'/move_base/NavfnROS/plan', Path, self.global_path_callback)
self.local_path_sub = rospy.Subscriber(
'/move_base/DWAPlannerROS/local_plan', Path, self.local_path_callback)
self.map_data = None
self.global_path = None
self.local_path = None
def setup_sensors(self):
"""设置传感器"""
# 激光扫描
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
self.latest_scan = None
# 传感器状态检查
rospy.Timer(rospy.Duration(1.0), self.check_sensors)
def setup_recovery(self):
"""设置恢复行为"""
# 清除代价地图服务
self.clear_costmaps_srv = rospy.ServiceProxy(
'/move_base/clear_costmaps', Empty)
# 恢复行为计数
self.recovery_attempts = 0
self.max_recovery_attempts = 3
def amcl_pose_callback(self, msg):
"""AMCL位姿回调"""
self.current_pose = msg.pose.pose
# 计算定位置信度
covariance = np.array(msg.pose.covariance).reshape(6, 6)
position_variance = covariance[0, 0] + covariance[1, 1]
if position_variance < 0.1:
self.localization_confidence = 1.0
elif position_variance < 0.5:
self.localization_confidence = 0.5
else:
self.localization_confidence = 0.1
if self.localization_confidence > 0.5:
self.components['localization'] = True
def map_callback(self, msg):
"""地图回调"""
self.map_data = msg
self.components['map'] = True
rospy.loginfo("Map received")
def scan_callback(self, msg):
"""激光扫描回调"""
self.latest_scan = msg
self.components['sensors'] = True
def global_path_callback(self, msg):
"""全局路径回调"""
self.global_path = msg
def local_path_callback(self, msg):
"""局部路径回调"""
self.local_path = msg
def check_sensors(self, event):
"""检查传感器状态"""
if self.latest_scan is None:
self.components['sensors'] = False
rospy.logwarn("No laser scan data")
# 检查扫描数据质量
if self.latest_scan:
valid_ranges = [r for r in self.latest_scan.ranges
if r > self.latest_scan.range_min and
r < self.latest_scan.range_max]
if len(valid_ranges) < len(self.latest_scan.ranges) * 0.5:
rospy.logwarn("Poor scan quality")
def system_check(self):
"""系统检查"""
all_ready = all(self.components.values())
if not all_ready:
rospy.logwarn("System not ready:")
for component, status in self.components.items():
if not status:
rospy.logwarn(f" - {component}: NOT READY")
return all_ready
def navigate_to_goal(self, goal_pose):
"""导航到目标"""
if not self.system_check():
rospy.logerr("System not ready for navigation")
return False
# 创建目标
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = goal_pose
# 发送目标
self.move_base_client.send_goal(goal)
self.state = 'navigating'
return True
def cancel_navigation(self):
"""取消导航"""
self.move_base_client.cancel_all_goals()
self.state = 'idle'
rospy.loginfo("Navigation cancelled")
# 状态机函数
def idle_state(self):
"""空闲状态"""
pass
def localizing_state(self):
"""定位状态"""
if self.localization_confidence > 0.8:
self.state = 'idle'
rospy.loginfo("Localization complete")
def planning_state(self):
"""规划状态"""
if self.global_path is not None:
self.state = 'navigating'
def navigating_state(self):
"""导航状态"""
state = self.move_base_client.get_state()
if state == actionlib.GoalStatus.SUCCEEDED:
self.state = 'reached'
self.recovery_attempts = 0
rospy.loginfo("Goal reached!")
elif state == actionlib.GoalStatus.ABORTED:
self.state = 'recovering'
rospy.logwarn("Navigation aborted, attempting recovery")
def recovering_state(self):
"""恢复状态"""
if self.recovery_attempts < self.max_recovery_attempts:
# 执行恢复行为
self.execute_recovery()
self.recovery_attempts += 1
self.state = 'navigating'
else:
self.state = 'idle'
rospy.logerr("Max recovery attempts reached")
def reached_state(self):
"""到达状态"""
rospy.sleep(1.0)
self.state = 'idle'
def execute_recovery(self):
"""执行恢复行为"""
rospy.loginfo(f"Executing recovery behavior {self.recovery_attempts + 1}")
# 清除代价地图
try:
self.clear_costmaps_srv()
rospy.loginfo("Costmaps cleared")
except:
rospy.logwarn("Failed to clear costmaps")
# 旋转恢复
if self.recovery_attempts > 0:
self.rotate_recovery()
def rotate_recovery(self):
"""旋转恢复"""
cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# 旋转360度
cmd = Twist()
cmd.angular.z = 0.5
duration = 2 * np.pi / 0.5 # 完整旋转时间
start_time = rospy.Time.now()
rate = rospy.Rate(10)
while (rospy.Time.now() - start_time).to_sec() < duration:
cmd_vel_pub.publish(cmd)
rate.sleep()
# 停止
cmd_vel_pub.publish(Twist())
def main_loop(self):
"""主循环"""
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# 执行当前状态
if self.state in self.state_machine:
self.state_machine[self.state]()
rate.sleep()
def main():
"""主函数"""
nav_system = AutonomousNavigationSystem()
# 示例：导航到目标
goal_pose = Pose()
goal_pose.position.x = 5.0
goal_pose.position.y = 5.0
goal_pose.orientation.w = 1.0
nav_system.navigate_to_goal(goal_pose)
rospy.spin()
if __name__ == '__main__':
main()11. 总结与最佳实践
11.1 本文总结
通过本文的学习，你已经掌握了：
- ✅ ROS导航栈架构：理解导航系统的整体结构
- ✅ move_base框架：掌握导航核心节点的使用
- ✅ 代价地图机制：配置和优化代价地图
- ✅ 全局路径规划：实现A*、Dijkstra等算法
- ✅ 局部路径规划：掌握DWA等局部规划器
- ✅ 恢复行为：处理导航失败情况
- ✅ 参数调优：优化导航性能
- ✅ 多目标导航：管理多个导航目标
- ✅ 多机器人协调：实现多机器人导航
- ✅ 完整系统：构建完整的导航系统
11.2 最佳实践
| 方面 | 建议 | 原因 |
|---|---|---|
| 参数调优 | 从保守参数开始 | 确保安全性 |
| 代价地图 | 适当的膨胀半径 | 平衡安全和通过性 |
| 规划频率 | 全局1Hz，局部10Hz | 平衡计算负载 |
| 恢复行为 | 逐级升级策略 | 提高成功率 |
| 传感器 | 多传感器融合 | 提高鲁棒性 |
| 目标容差 | 合理设置容差 | 避免无法到达 |
11.3 常见问题解决
-
机器人原地打转
- 检查代价地图配置
- 调整振荡参数
- 增加路径跟踪权重
-
无法到达目标
- 检查目标是否在障碍物中
- 调整目标容差
- 检查代价地图膨胀
-
路径规划失败
- 确认地图完整性
- 检查起点和终点
- 调整规划器参数
-
导航速度过慢
- 增加最大速度限制
- 优化加速度参数
- 减少安全裕度
11.4 下一步学习
在下一篇文章中，我们将学习：
- SLAM建图：同时定位与地图构建
- gmapping：基于粒子滤波的SLAM
- cartographer：Google的SLAM解决方案
- 建图优化：提高地图质量
版权声明：本文为原创文章，转载请注明出处
💡 学习建议：导航是移动机器人最核心的功能之一。建议先在仿真环境中充分测试，熟悉各个参数的影响。实际部署时，要根据具体的机器人和环境特点进行细致的调优。记住，没有通用的最佳参数，需要根据实际情况不断调整。
下一篇预告：《【ROS1从入门到精通】第13篇：SLAM建图（同时定位与地图构建）》
敬请期待！