我在做导航任务时总结了一下几项导航方式（均为基于目标点坐标的导航方式）
代码方面不做过多解释，可以查看我的上一篇文章
导航功能包的用法：
一，同步导航
同步导航是一种阻塞式执行方式。客户端发送目标后，程序会阻塞(客户端发送目标后，程序会阻塞，直到任务完成（成功或失败）,在任务完成之前，在任务完成之前，客户端无法执行其他操作，也无法实时获取任务的反馈)，也就是说小车只有导航到目标点后，客户端Client才会发送下一个目标点，这就会使小车每到达一个目标点就停顿1s左右的时间(发送下一个目标点并规划路径)。
同步导航不支持回调函数，因此无法实时获取任务的反馈（feedback）。任务状态（如 PENDING、ACTIVE、SUCCEEDED 等）只能在任务完成后通过 client.get_state() 获取。
同步导航程序会调用 client.wait_for_result()，阻塞直到任务完成，这是本质。
工作流程：
-
客户端发送目标点 1。
-
程序阻塞，等待目标点 1 完成。
-
目标点 1 完成后，客户端发送目标点 2。
-
程序再次阻塞，等待目标点 2 完成。
-
重复上述过程，直到所有目标点完成。
它的实现比较简单，直接给代码了
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Point, Quaternion
class SyncNavigationClient:
def __init__(self):
# 初始化 Action Client
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("等待 move_base Action Server 启动...")
self.client.wait_for_server()
rospy.loginfo("move_base Action Server 已启动！")
def send_goal(self, target_pose):
"""
发送导航目标（同步）
:param target_pose: 目标点的位置和方向（字典格式）
"""
# 构造 MoveBaseGoal 对象
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position = Point(**target_pose['position'])
goal.target_pose.pose.orientation = Quaternion(**target_pose['orientation'])
# 发送目标并等待结果
rospy.loginfo(f"发送目标点: {target_pose}")
self.client.send_goal(goal)
self.client.wait_for_result() # 阻塞，直到任务完成
# 检查任务状态
if self.client.get_state() == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("导航成功！")
else:
rospy.loginfo("导航失败！")
if __name__ == '__main__':
try:
# 初始化 ROS 节点
rospy.init_node('sync_navigation_client')
# 创建 SyncNavigationClient 对象
navigator = SyncNavigationClient()
# 定义目标点
target_pose = {
'position': {'x': 1.0, 'y': 0.5, 'z': 0.0},
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
}
# 发送目标点
navigator.send_goal(target_pose)
except rospy.ROSInterruptException:
rospy.logerr("程序被中断！")如果是多个目标点需要导航，主函数main中可以采用for循环的方式。一般多个目标点都采用一个文件统一存储，这里我使用的是yaml文件来存储多个目标点，load_target_points(file_path)函数用来从yaml文件读取内容。同时，下方代码还实现了在特定目标点执行任务，执行完任务继续导航的功能
# 加载导航目标点
def load_target_points(file_path):
with open(file_path, 'r') as f:
return yaml.safe_load(f)
if __name__ == '__main__':
try:
# 初始化 ROS 节点
rospy.init_node('sync_navigation_client')
#加载导航目标点
yaml_path = os.path.join(os.path.dirname(__file__), '../param/target_points.yaml')
target_points = load_target_points(yaml_path)['target_points']
# 创建 SyncNavigationClient 对象
navigator = SyncNavigationClient()
# 遍历目标点并导航
for idx, (point_name, point_data) in enumerate(target_points.items(), start=1):
rospy.loginfo(f"导航到目标点: {point_name} (第 {idx} 个点)")
# 发送目标点
if navigator.send_goal(point_data):
rospy.loginfo(f"到达 {point_name}！")
# 如果是第 i 个或第 j 个点，停下来执行任务
if idx == i or idx == j:
rospy.loginfo(f"在 {point_name} 执行任务...")
# 需要执行的任务
else:
rospy.loginfo(f"无任务,继续导航到下一个点。")
else:
rospy.logerr(f"导航到 {point_name} 失败！")
break
except rospy.ROSInterruptException:
rospy.logerr("程序被中断！")二，异步导航
与同步导航相反，异步导航是非阻塞式执行，客户端发送目标后，程序不会阻塞，可以继续执行其他操作，可以通过回调函数处理任务的状态和反馈。
工作流程
-
客户端发送目标点 1。
-
程序继续执行，不阻塞。
-
客户端通过回调函数监控任务状态和反馈。
-
当目标点 1 完成后，回调函数触发，客户端发送目标点 2。
-
程序继续执行，不阻塞。
-
重复上述过程，直到所有目标点完成。
程序不堵塞是异步和同步的本质区别，下面将从导航的角度提出在导航过程中的问题：
问题一：客户端发送目标点1后何时发送目标点2？
问题二：如果目标点 1 未到达就发送目标点 2，小车是会继续沿着当前位置到目标点1的位置规划路径还是直接规划从当前位置到目标点2的位置？
针对以上问题，下面给出解释：
解释一：对于何时发送目标点2，我们首先要知道异步发送它的程序是不堵塞的，也就是说，在小车运动时，程序会不断进入回调函数goal_done_callback中执行其中的程序，所以顺理成章我们可以在回调函数里设置一些判断条件，比如说如果到达目标1就执行一些操作(也可以是发送目标点2这个操作，不过这就与同步导航效果一样了)，这个判断条件可以是距离目标点1 0.1m时发送目标点2这样小车就可以实现不停顿运行(需要编写函数实时监控小车位置，会在下方代码示例中有)，并且会经过每个目标点。同时也可以设置到达目标点4时执行停下执行相关任务等等，可以灵活使用。
解释二：对于第二个问题，如果目标点 1 未到达就发送目标点 2，小车的具体行为取决于 Action Server 的实现，我们导航使用的Action Server一般是move_base。所以在这里，我们需要了解move_base它的行为之一单目标点处理，也就是move_base一次只处理一个目标点，如果客户端发送目标点 1 后，在目标点 1 完成之前发送目标点 2，move_base 会 取消目标点 1，并开始处理目标点 2。
以下段代码为例，它的初始化以及send_goal部分与同步导航相同，不同点就是多了回调函数的使用，回调函数中的逻辑是在到达每个目标点后再发送下一个目标点，所以执行效果和同步导航一样
class MultiGoalNavigatorAsynchronization:
def __init__(self):
# 连接到move_base动作服务器
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
self.target_points = []
self.current_index = 0
def set_target_points(self, target_points):
self.target_points = target_points
self.current_index = 0
def send_goal(self, target_pose, done_callback=None):
# 构造导航目标
goal = MoveBaseGoal() # 创建一个 MoveBaseGoal 对象
goal.target_pose.header.frame_id = "map" # 设置目标位姿的参考坐标系为 "map"
goal.target_pose.pose.position = Point(**target_pose['position']) # 目标的位置（Point 类型，包含 x, y, z 坐标）。
goal.target_pose.pose.orientation = Quaternion(**target_pose['orientation']) # 目标的方向（Quaternion 类型，表示旋转）。
# 异步发送目标点
self.client.send_goal(goal, done_cb=done_callback)
rospy.loginfo(f"目标点已发送: {target_pose}")
def start_navigation(self):
if self.target_points:
self.send_goal(self.target_points[self.current_index], self.goal_done_callback)
# 回调函数
def goal_done_callback(self, status, result):
"""
目标点完成时的回调函数
:param status: 目标点的状态
:param result: 目标点的结果
"""
if status == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo(f"成功到达目标点: {self.target_points[self.current_index]}")
# 如果是第 4 个点，执行任务(索引从0开始，所以第4个点的索引是3)
if self.current_index == 3:
rospy.loginfo("到达第4个目标点，开始执行任务...")
#elif idx == 5:
#elif idx == 9:
else:
rospy.loginfo(f"无任务,继续导航到下一个点。")
self.current_index += 1
if self.current_index < len(self.target_points):
self.send_goal(self.target_points[self.current_index], self.goal_done_callback)
else:
rospy.logerr(f"导航到目标点 {self.target_points[self.current_index]} 失败！")
else:
rospy.logerr(f"导航到 {self.current_index} 失败！")下面一段代码，就是在距离目标点0.1m时发送下一个目标点的类
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Point, Quaternion, PoseStamped, PoseWithCovarianceStamped
from actionlib import GoalStatus
class MultiGoalNavigatorAsynchronization:
def __init__(self):
# 连接到move_base动作服务器
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
self.target_points = [] # 存储目标点
self.current_index = 0 # 当前目标点索引
self.current_goal = None # 当前目标点
self.current_pose = None # 小车当前位置
self.threshold = 0.5 # 距离阈值（0.1 米）
# 订阅小车当前位置
rospy.loginfo("正在订阅 /amcl_pose 话题")
self.pose_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.pose_callback)
rospy.loginfo("已订阅 /amcl_pose 话题")
def set_target_points(self, target_points):
"""
设置目标点列表
:param target_points: 目标点列表（字典格式）
"""
self.target_points = target_points
self.current_index = 0
def send_goal(self, target_pose, done_callback=None):
"""
发送导航目标
:param target_pose: 目标点的位置和方向（字典格式）
:param done_callback: 任务完成时的回调函数
"""
# 构造导航目标
goal = MoveBaseGoal() # 创建一个 MoveBaseGoal 对象
goal.target_pose.header.frame_id = "map" # 设置目标位姿的参考坐标系为 "map"
goal.target_pose.header.stamp = rospy.Time.now() # 为导航目标点的时间戳赋值
goal.target_pose.pose.position = Point(**target_pose['position']) # 目标的位置（Point 类型，包含 x, y, z 坐标）。
goal.target_pose.pose.orientation = Quaternion(**target_pose['orientation']) # 目标的方向（Quaternion 类型，表示旋转）。
# 异步发送目标点
self.client.send_goal(goal, done_cb=done_callback)
rospy.loginfo(f"目标点已发送: {target_pose}")
self.current_goal = goal # 记录当前目标点
def start_navigation(self):
"""
开始导航
"""
if self.target_points:
self.send_goal(self.target_points[self.current_index], self.goal_done_callback)
def pose_callback(self, msg):
"""
小车当前位置的回调函数
:param msg: 小车当前位置（PoseStamped 类型）
"""
try:
rospy.loginfo("pose_callback已触发")
self.current_pose = msg.pose.pose # 更新小车当前位置
# 检查是否接近当前目标点
if self.current_goal and self.current_pose:
distance = self.calculate_distance(self.current_pose.position, self.current_goal.target_pose.pose.position)
if distance < self.threshold:
rospy.loginfo(f"距离目标点 {self.current_index + 1} 小于 {self.threshold} 米，准备发送下一个目标点。")
self.send_next_goal() # 接近目标点则接着发送下一个目标点
except Exception as e:
rospy.logerr(f"pose_callback 发生异常: {e}")
def calculate_distance(self, pose1, pose2):
"""
计算两个位置之间的距离
:param pose1: 位置 1（Point 类型）
:param pose2: 位置 2（Point 类型）
:return: 距离（米）
"""
dx = pose1.x - pose2.x
dy = pose1.y - pose2.y
distance = (dx ** 2 + dy ** 2) ** 0.5
rospy.loginfo(f"当前位置与目标位置的距离: {distance} 米")
return distance
def send_next_goal(self):
"""
发送下一个目标点
"""
rospy.loginfo("send_next_goal 被调用")
self.current_index += 1
if self.current_index < len(self.target_points):
rospy.loginfo(f"发送目标点 {self.current_index + 1}。")
self.send_goal(self.target_points[self.current_index], self.goal_done_callback)
else:
rospy.loginfo("所有目标点已完成！")
# 回调函数
def goal_done_callback(self, status, result):
"""
目标点完成时的回调函数
:param status: 目标点的状态
:param result: 目标点的结果
"""
if status == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo(f"成功到达目标点: {self.target_points[self.current_index]}")
# 如果是第 4 个点，执行任务(索引从0开始，所以第4个点的索引是3)
if self.current_index == 3:
rospy.loginfo("到达第4个目标点，开始执行任务...")
rospy.sleep(5)
#elif idx == 5:
#elif idx == 9:
else:
rospy.loginfo(f"无任务,继续导航到下一个点。")
else:
rospy.logerr(f"导航到 {self.current_index} 失败！")
if __name__ == '__main__':
try:
# 初始化 ROS 节点
rospy.init_node('multi_goal_navigator')
# 创建 MultiGoalNavigatorAsynchronization 对象
navigator = MultiGoalNavigatorAsynchronization()
# 定义目标点列表
target_points = [
{'position': {'x': 1.0, 'y': 0.5, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}},
{'position': {'x': 2.0, 'y': 1.0, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}},
{'position': {'x': 3.0, 'y': 1.5, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}},
{'position': {'x': 4.0, 'y': 2.0, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}},
{'position': {'x': 5.0, 'y': 2.5, 'z': 0.0}, 'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}}
]
# 设置目标点列表
navigator.set_target_points(target_points)
# 开始导航
navigator.start_navigation()
# 保持节点运行，以便接收回调
rospy.spin()
except rospy.ROSInterruptException:
rospy.logerr("程序被中断！")补充：保存导航目标点
roslaunch运行导航包(启动move_base，rviz)。如下图
然后再打开一个终端运行程序goal_server.py文件。运行成功后就可以使用rviz的2D New Goal在地图上选择要到达的目标点，每标一个点小车就会规划路径移动过去同时目标点坐标角度等信息会保存在target_points.yaml文件。示例代码如下
goal_server.py文件
#!/usr/bin/env python3
import rospy
import yaml
from geometry_msgs.msg import PoseStamped
from Nav import GoalSaver
if __name__ == '__main__':
rospy.init_node('goal_saver')
yaml_file = rospy.get_param('~yaml_file', '/home/iflytek/ucar_ws/src/main/param/target_points.yaml') # 目标点保存的路径
goal_saver = GoalSaver(yaml_file)
rospy.spin()Nav.py文件
import rospy
import os
import yaml
from geometry_msgs.msg import Point, Quaternion, PoseStamped
# 在RViz中使用2D Nav Goal工具选择目标点，数据会发W布到/move_base_simple/goal话题。
# GoalSaver类用于创建一个ROS节点订阅/move_base_simple/goal话题，将目标点保存到YAML文件作为导航目标点
# 通过2D New Goal打点来记录目标点位置，保存到param/target_points.yaml中
class GoalSaver:
def __init__(self, yaml_file):
self.yaml_file = yaml_file
self.target_points = {} # 用于存储目标点的字典
self.point_counter = 1 # 用于生成point1, point2, point3等键
self.subscriber = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.goal_callback)
def goal_callback(self, msg):
# 生成目标点的键（如point1, point2, point3等）
point_key = f"point{self.point_counter}"
# 提取目标点的位置和方向
goal = {
'position': {
'x': msg.pose.position.x,
'y': msg.pose.position.y,
'z': msg.pose.position.z
},
'orientation': {
'x': msg.pose.orientation.x,
'y': msg.pose.orientation.y,
'z': msg.pose.orientation.z,
'w': msg.pose.orientation.w
}
}
# 将目标点添加到字典中
self.target_points[point_key] = goal
self.point_counter += 1 # 递增计数器
# 保存到YAML文件
self.save_to_yaml()
def save_to_yaml(self):
# 将目标点字典包装在顶层键'target_points'中
data_to_save = {'target_points': self.target_points}
# 写入YAML文件
with open(self.yaml_file, 'w') as file:
yaml.dump(data_to_save, file, default_flow_style=False)
rospy.loginfo(f"Goal saved to {self.yaml_file}")保存的文件target_points.yaml内容示例：
target_points:
point1:
orientation:
w: 0.8718495500348453
x: 0.0
y: 0.0
z: 0.48977378666486204
position:
x: 1.710163950920105
y: 0.12763656675815582
z: 0.0
point2:
orientation:
w: 0.04053538378629026
x: 0.0
y: 0.0
z: 0.9991781035737813
position:
x: 1.4441925287246704
y: 0.42180135846138
z: 0.0
point3:
orientation:
w: 0.018045355868244944
x: 0.0
y: 0.0
z: -0.9998371693088772
position:
x: 0.9935807585716248
y: 0.43682125210762024
z: 0.0