引言
在ROS1中，激光雷达在自主导航系统中起着至关重要的作用。通过雷达提供的数据，机器人可以感知周围环境，识别障碍物，并规划路径。本文将详细介绍雷达在ROS中的应用，涉及雷达相关的所有话题，以及如何使用这些话题实现单点导航和多点导航，尤其是在一般逻辑无法实现自主导航的情况下如何选用可行的替代方案实现单点导航和多点导航。
问题1：小车无法及时获取到达导航点状态信息GoalStatus.SUCCEEDED
问题起因
在固定的场地里面希望实现多点导航，给定导航点的过程中，小车导航程序所在终端反馈正常，但是将所有导航点集成到发点程序的时候小车每一步路径规划都出现了问题，尤其是出现了无法规划路径的严重错误，尝试过了很多遍重新给导航点，修正地图但还是没有解决。
问题分析
-
导航点之间的连通性
检查路径连续性：确保导航点之间的路径是连续的，并且不存在不可逾越的障碍物。有时候两个点之间看似可以通过，但实际上由于某些细微的障碍物导致路径不可行。
检查路径长度：如果两个导航点之间的距离过长，可能需要增加中间的参考点来确保路径规划的可行性。
-
路径规划器的全局视图
全局路径规划器配置：确认全局路径规划器（如move_base的global_planner）是否正确配置。有时路径规划器可能需要全局地图信息来进行决策，如果地图信息不完整或不准确，会导致规划失败。
重置规划器状态：尝试在每个新的导航点之前重置路径规划器的状态，确保它不会受到之前规划的影响。
-
局部路径规划与执行
局部路径规划器配置：确认局部路径规划器（如move_base的local_planner）是否正确配置。局部路径规划器负责实时调整路径以避开障碍物，如果其参数设置不当，可能导致路径规划失败。
避障算法：检查避障算法是否能够有效工作。如果避障算法未能正确检测或规避障碍物，可能导致路径规划失败。 -
导航点顺序
检查导航点顺序：确保导航点的顺序合理，有时候导航点的顺序不同可能导致不同的结果。例如，如果从点A到点B再到点C比先从A到C再到B更容易，那么路径规划结果也会不同。
动态路径调整：考虑在多点导航中加入动态路径调整机制，根据实际情况调整路径顺序或增加临时路径点。
-
路径成本计算
成本函数：检查路径成本函数是否合理。成本函数用于评估不同路径的成本，如果设置不当，可能导致选择次优路径或根本无法找到路径。
权重调整：调整路径规划中的权重参数，比如障碍物避让的优先级、路径平滑度等，看看是否能改善路径规划效果。
-
系统同步问题
检查同步：确保所有系统的时钟同步，特别是传感器数据的时间戳。不同步的数据可能导致路径规划器无法正确处理信息。
解决方向1
- 逐点测试：先单独测试每个导航点，确保每个点都能成功到达。
- 逐步添加：从两个点开始，逐渐增加导航点数量，观察何时开始出现问题。
- 可视化工具：使用ROS的可视化工具（如rviz）来观察路径规划的过程。
- 参数调整：适当调整路径规划器的相关参数，看看是否能改善结果。
以上方法均需要较大的时间成本，不过优点是不需要改变原有的逻辑，同时能较好的解决问题。
以下是我在原逻辑上做的改动：
（1）使用Python和actionlib库
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import GoalStatus
def move_to_goal(x, y, z):
# 创建SimpleActionClient，连接到move_base
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# 等待服务器启动
client.wait_for_server()
# 设置目标位置
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 = z
goal.target_pose.pose.orientation.w = 1.0
# 发送目标
client.send_goal(goal)
# 等待结果
while not client.wait_for_result(rospy.Duration(10.0)):
if client.get_state() == GoalStatus.PREEMPTED:
rospy.loginfo('The goal was preempted by the user')
client.cancel_goal()
break
elif client.get_state() == GoalStatus.ABORTED:
rospy.loginfo('The goal was aborted')
break
elif client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Reached the goal!')
# 导航成功，执行拍照识别
take_photo_and_recognize()
break
else:
rospy.loginfo('Waiting for the move_base action to complete for {}...'.format(rospy.get_time()))
def take_photo_and_recognize():
# 拍照并识别的逻辑
rospy.loginfo("Taking photo and recognizing...")
if __name__ == '__main__':
try:
rospy.init_node('move_and_recognize', anonymous=True)
move_to_goal(1.0, 2.0, 0.0)
except rospy.ROSInterruptException:
rospy.loginfo("Navigation and recognition process finished.")
执行之后发现导航终端依旧会出现路径规划失败，无法规划路径等问题，于是我再次改造原来的逻辑：
#!/usr/bin/env python3
# encoding=utf-8
import rospy
from actionlib import SimpleActionClient
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped, Quaternion
import subprocess
import os
from actionlib_msgs.msg import GoalStatus
env_name = 'yolov5'
class Waypoint:
def __init__(self, frame_id, x, y, z=0.0, qx=0.0, qy=0.0, qz=0.0, qw=1.0):
self.pose = PoseStamped()
self.pose.header.frame_id = frame_id
self.pose.header.stamp = rospy.Time.now()
self.pose.pose.position.x = x
self.pose.pose.position.y = y
self.pose.pose.position.z = z
self.pose.pose.orientation = Quaternion(qx, qy, qz, qw)
def to_move_base_goal(self):
goal = MoveBaseGoal()
goal.target_pose = self.pose
return goal
def take_photo_and_recognize(env_name):
# 这添加调用拍照和识别的代码，因为是测试故而直接使用打印测试可行性
print(f"Taking photo and recognizing at {env_name}...")
def goto_waypoints(waypoints, indices_to_shoot):
client = SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
all_succeeded = True
for index, waypoint in enumerate(waypoints):
goal = waypoint.to_move_base_goal()
rospy.loginfo("Going to waypoint: (%.2f, %.2f)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
client.send_goal(goal)
wait = client.wait_for_result(rospy.Duration(10.0))
if not wait:
rospy.logerr("Action server not available for waypoint: (%.2f, %.2f)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
all_succeeded = False
continue
status = client.get_state()
if status == GoalStatus.SUCCEEDED:
rospy.loginfo("Reached waypoint successfully!")
if index in indices_to_shoot:
take_photo_and_recognize(env_name)
else:
rospy.logerr("Failed to reach waypoint: (%.2f, %.2f)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
all_succeeded = False
if __name__ == '__main__':
rospy.init_node('goto_waypoints_node')
waypoints = [
Waypoint("map", 2.6532340049743652, 0.003983676433563232, 0.0, 0.0, 0.0, 0.9999788050546564, -0.006510717430615279)
]
indices_to_shoot=[0]
if(goto_waypoints(waypoints, indices_to_shoot)):
print("Nav Successful")
else:
print("Wait for begin")
至此原来出现的问题还是没有解决，由于不想要继续调整地图，因此我选择使用其他话题实现这个功能，其中路径规划器比较垃圾是让我做出这一决定的关键因素。
另外的解决方向：使用其他话题实现同样的功能
在终端查看话题的时候，我发现了一下话题：
/move_base/DWAPlannerROS/cost_cloud
/move_base/DWAPlannerROS/global_plan
/move_base/DWAPlannerROS/local_plan
/move_base/DWAPlannerROS/parameter_descriptions
/move_base/DWAPlannerROS/parameter_updates
/move_base/DWAPlannerROS/trajectory_cloud
/move_base/GlobalPlanner/parameter_descriptions
/move_base/GlobalPlanner/parameter_updates
/move_base/GlobalPlanner/plan
/move_base/GlobalPlanner/potential
/move_base/cancel
/move_base/current_goal
/move_base/feedback
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/global_costmap/footprint
/move_base/global_costmap/inflation_layer/parameter_descriptions
/move_base/global_costmap/inflation_layer/parameter_updates
/move_base/global_costmap/obstacle_layer/parameter_descriptions
/move_base/global_costmap/obstacle_layer/parameter_updates
/move_base/global_costmap/parameter_descriptions
/move_base/global_costmap/parameter_updates
/move_base/global_costmap/static_layer/parameter_descriptions
/move_base/global_costmap/static_layer/parameter_updates
/move_base/goal
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base/local_costmap/footprint
/move_base/local_costmap/inflation_layer/parameter_descriptions
/move_base/local_costmap/inflation_layer/parameter_updates
/move_base/local_costmap/obstacle_layer/parameter_descriptions
/move_base/local_costmap/obstacle_layer/parameter_updates
/move_base/local_costmap/parameter_descriptions
/move_base/local_costmap/parameter_updates
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal
话题选择
上述有不少的话题可以用来实现自主导航的功能，具体效果需要考虑。
- /move_base/goal
用途：发送目标位置给move_base节点，用于触发路径规划和导航动作。
实现方式：通过此话题发送actionlib_msgs/GoalID类型的消息来设定目标位置。 - /move_base_simple/goal
用途：简化版的目标发送话题，直接发送目标点而不需通过动作客户端。
实现方式：发送geometry_msgs/PoseStamped类型的消息，其中包含目标点的位置和朝向。 - /move_base/GlobalPlanner/plan
用途：发布全局路径计划。
实现方式：监听此话题可以获取全局路径计划信息，类型为nav_msgs/Path。 - /move_base/DWAPlannerROS/global_plan
用途：发布全局路径。
实现方式：同样监听此话题可以获取全局路径计划信息，类型为nav_msgs/Path。 - /move_base/DWAPlannerROS/local_plan
用途：发布局部路径。
实现方式：监听此话题可以获取局部路径计划信息，类型为nav_msgs/Path。 - /move_base/current_goal
用途：当前正在执行的目标。
实现方式：监听此话题可以获取当前正在执行的目标信息，类型为geometry_msgs/PoseStamped。 - /move_base/status
用途：提供当前导航任务的状态信息。
实现方式：监听此话题可以了解当前导航任务的状态，类型为actionlib_msgs/GoalStatusArray。 - /move_base/result
用途：提供导航任务的结果信息。
实现方式：监听此话题可以获取导航任务的结果，类型为move_base_msgs/MoveBaseActionResult。 - /move_base/feedback
用途：提供导航任务的反馈信息。
实现方式：监听此话题可以获取导航任务的实时反馈，类型为move_base_msgs/MoveBaseActionFeedback。
上述话题原则上均可实现单点导航和多点导航，但应用到实际过程中，其实用的比较多的是话题/move_base_simple/goal，也可以使用话题/move_base/feedback。
基于/move_base_simple/goal
我们给出使用/move_base_simple/goal实现自主导航的功能，示例代码如下：
#!/usr/bin/env python3
# encoding=utf-8
import rospy
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler
def move_to_point(x, y, theta):
pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
rospy.init_node('navigation_node', anonymous=True)
goal_pose = PoseStamped()
goal_pose.header.frame_id = "map"
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position.x = x
goal_pose.pose.position.y = y
goal_pose.pose.position.z = 0.0
# 设置朝向
q = quaternion_from_euler(0, 0, theta)
goal_pose.pose.orientation.x = q[0]
goal_pose.pose.orientation.y = q[1]
goal_pose.pose.orientation.z = q[2]
goal_pose.pose.orientation.w = q[3]
pub.publish(goal_pose)
if __name__ == '__main__':
try:
move_to_point(1.0, 2.0, 0.0) # 目标点坐标和朝向
rospy.loginfo("Goal sent successfully")
except rospy.ROSInterruptException:
rospy.loginfo("Navigation terminated")
上述代码中的move_to_point(1.0, 2.0, 0.0)分别对应了目标点坐标和朝向，但是我们在获取导航点时候一般使用的是下面的指令：
rostopic echo /move_base/goal
获取到的导航点信息一般是类似这样的：
"frame_id": "map",
"x": 2.768982219696045,
"y": 0.0,
"z": 0.0,
"qx": 0.0,
"qy": 0.0,
"qz": 0.711578113138931,
"qw": 0.7026069946290306
我们需要将其转化成示例代码中的形式，这可以通过编写脚本实现，这里暂不做阐述。
基于/move_base/feedback
使用该话题可以帮助实时监控导航任务的状态，这对于实现多点导航非常有用，因为它允许在每个目标点之间进行状态检查和逻辑控制。
基于该话题实现多点导航的主要流程包括创建一个ROS节点，然后订阅/move_base/feedback话题，并发送目标点给move_base节点，详细示例代码如下：
#!/usr/bin/env python3
# encoding=utf-8
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
from geometry_msgs.msg import PoseStamped, Quaternion
from actionlib_msgs.msg import GoalStatus
class MultiPointNavigation:
def __init__(self):
# 初始化ROS节点
rospy.init_node('multi_point_navigation_node')
# 创建一个Action客户端来与move_base节点通信
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
# 订阅/move_base/feedback话题，用于监控导航任务的状态
self.feedback_sub = rospy.Subscriber('/move_base/feedback', MoveBaseActionFeedback, self.feedback_callback)
# 定义目标点列表
self.waypoints = [
self.create_waypoint("map", 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0),
self.create_waypoint("map", 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0),
self.create_waypoint("map", 3.0, 3.0, 0.0, 0.0, 0.0, 0.0, 1.0)
]
# 当前正在处理的目标点索引
self.current_index = 0
# 发送第一个目标点
self.send_next_goal()
def create_waypoint(self, frame_id, x, y, z=0.0, qx=0.0, qy=0.0, qz=0.0, qw=1.0):
"""创建一个目标点"""
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation = Quaternion(qx, qy, qz, qw)
return pose
def send_next_goal(self):
"""发送下一个目标点"""
if self.current_index < len(self.waypoints):
goal = MoveBaseGoal()
goal.target_pose = self.waypoints[self.current_index]
self.client.send_goal(goal, done_cb=self.goal_done_callback)
self.current_index += 1
else:
rospy.loginfo("All waypoints reached.")
def feedback_callback(self, feedback):
"""反馈回调函数，用于处理导航任务的状态反馈"""
status = feedback.status.status
if status == GoalStatus.ACTIVE or status == GoalStatus.PENDING:
#不做任何处理
pass
elif status == GoalStatus.SUCCEEDED:
rospy.loginfo("Reached waypoint: (%.2f, %.2f)",
feedback.feedback.base_position.pose.position.x,
feedback.feedback.base_position.pose.position.y)
# 如果任务成功，发送下一个目标点
self.send_next_goal()
else:
rospy.logwarn("Failed to reach waypoint: (%.2f, %.2f)",
feedback.feedback.base_position.pose.position.x,
feedback.feedback.base_position.pose.position.y)
def goal_done_callback(self, state, result):
"""目标完成回调函数，用于处理目标完成后的状态"""
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal reached successfully.")
else:
rospy.logwarn("Goal failed with state: %s", state)
if __name__ == '__main__':
try:
navigator = MultiPointNavigation()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation terminated")
上述代码首先初始化ROS节点，并创建一个SimpleActionClient用于与move_base节点通信，订阅/move_base/feedback话题。
之后创建目标点，使用create_waypoint函数创建一个PoseStamped对象，表示一个目标点的位置和朝向，而后通过send_next_goal函数用于发送下一个目标点给move_base节点，并递增当前目标点索引。
在此过程中使用反馈回调，定义feedback_callback函数用于处理来自/move_base/feedback话题的状态反馈，并根据反馈的状态，决定是否发送下一个目标点，当目标完成回调，使用goal_done_callback函数用于处理目标完成后的状态。
由于上述代码使用状态码来判定是否到达导航点，实时状态又相对不可靠，为此我们对代码做出修改，添加距离阈值和角度阈值来实现自主导航过程中的到达导航点判定。以下是修改之后的代码：
#!/usr/bin/env python3
# encoding=utf-8
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
from geometry_msgs.msg import PoseStamped, Quaternion
from actionlib_msgs.msg import GoalStatus
import math
class MultiPointNavigation:
def __init__(self):
# 初始化ROS节点
rospy.init_node('multi_point_navigation_node')
# 创建一个Action客户端来与move_base节点通信
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
# 订阅/move_base/feedback话题，用于监控导航任务的状态
self.feedback_sub = rospy.Subscriber('/move_base/feedback', MoveBaseActionFeedback, self.feedback_callback)
# 定义目标点列表
self.waypoints = [
self.create_waypoint("map", 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0),
self.create_waypoint("map", 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0),
self.create_waypoint("map", 3.0, 3.0, 0.0, 0.0, 0.0, 0.0, 1.0)
]
# 当前正在处理的目标点索引
self.current_index = 0
# 定义距离和角度阈值
self.distance_threshold = 0.1 # 距离阈值，单位：米
self.angle_threshold = math.radians(5) # 角度阈值，单位：弧度 0.1rad ≈ 5.7°
# 发送第一个目标点
self.send_next_goal()
def create_waypoint(self, frame_id, x, y, z=0.0, qx=0.0, qy=0.0, qz=0.0, qw=1.0):
"""创建一个目标点"""
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation = Quaternion(qx, qy, qz, qw)
return pose
def send_next_goal(self):
"""发送下一个目标点"""
if self.current_index < len(self.waypoints):
goal = MoveBaseGoal()
goal.target_pose = self.waypoints[self.current_index]
self.client.send_goal(goal, done_cb=self.goal_done_callback)
self.current_index += 1
else:
rospy.loginfo("All waypoints reached.")
def feedback_callback(self, feedback):
"""反馈回调函数，用于处理导航任务的状态反馈"""
status = feedback.status.status
if status == GoalStatus.ACTIVE or status == GoalStatus.PENDING:
# 检查距离和角度
self.check_distance_and_angle(feedback)
elif status == GoalStatus.SUCCEEDED:
rospy.loginfo("Reached waypoint: (%.2f, %.2f)",
feedback.feedback.base_position.pose.position.x,
feedback.feedback.base_position.pose.position.y)
# 发送下一个目标点
self.send_next_goal()
else:
rospy.logwarn("Failed to reach waypoint: (%.2f, %.2f)",
feedback.feedback.base_position.pose.position.x,
feedback.feedback.base_position.pose.position.y)
def check_distance_and_angle(self, feedback):
"""检查当前位置与目标位置的距离和角度差异"""
target_pose = self.waypoints[self.current_index - 1].pose
current_pose = feedback.feedback.base_position.pose
# 计算距离差
distance_diff = math.sqrt(
(target_pose.position.x - current_pose.position.x)**2 +
(target_pose.position.y - current_pose.position.y)**2
)
# 计算角度差
angle_diff = self.quaternion_to_euler(current_pose.orientation)[-1] # 获取yaw角
target_angle = self.quaternion_to_euler(target_pose.orientation)[-1] # 获取目标yaw角
angle_diff = self.normalize_angle(target_angle - angle_diff) # 归一化角度
# 判断是否到达目标点
if distance_diff < self.distance_threshold and abs(angle_diff) < self.angle_threshold:
rospy.loginfo("Reached waypoint: (%.2f, %.2f)", target_pose.position.x, target_pose.position.y)
self.send_next_goal()
def quaternion_to_euler(self, quaternion):
"""将四元数转换为欧拉角"""
x, y, z, w = quaternion.x, quaternion.y, quaternion.z, quaternion.w
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z
def normalize_angle(self, angle):
"""归一化角度到[-pi, pi]区间"""
while angle > math.pi:
angle -= 2.0 * math.pi
while angle < -math.pi:
angle += 2.0 * math.pi
return angle
def goal_done_callback(self, state, result):
"""目标完成回调函数，用于处理目标完成后的状态"""
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal reached successfully.")
else:
rospy.logwarn("Goal failed with state: %s", state)
if __name__ == '__main__':
try:
navigator = MultiPointNavigation()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation terminated")
上述代码在创建和发送目标点之前定义了两个阈值，其中distance_threshold定义了距离阈值，默认为0.1米，angle_threshold定义了角度阈值，默认为5度，转换为弧度。
小结
通过以上话题的切换和逻辑的完善，我们能够逐渐实现自主导航，当然自主导航的过程需要不断调整参数以完善这一功能。尽管通过上述操作我们另辟蹊径实现了统一功能，但是精准的地图和表现良好的路径规划器对于高精度的自主导航是至关重要的。后续我将继续详细分析不同的建图算法，以求为高精度的自主导航打下坚实的基础。上文阐述有不到位的地方还请大家多多指教，下期再会！
标签：pose,goal,self,move,base,rospy,导航,ROS1,调试 From： https://blog.csdn.net/winner0111/article/details/141893353