ROS获取目标点导航完成状态（rospy）
文章目录
- ROS获取目标点导航完成状态
-
- 1. Action方式
- 2. Topic方式
- 3. 验证
- 4. 状态码取值
ROS获取目标点导航完成状态
1. Action方式
在ROS中，导航框架默认使用move_base，所以对于导航状态的获取往往需要往move_base的交互状态组建上考虑。
一种常见的方法是使用ROS的Action服务器（Action Server）和Action客户端（Action Client）来监视导航的状态。
首先，导航目标点的发布可以将使用rospy.Publisher，对于导航完成情况的状态可以使用Action的客户端实现，需要导入actionlib，使用actionlib.SimpleActionClient完成客户端的创建。
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
# 创建一个Action客户端
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
在使用Action客户端之前，需要等待Action服务器准备好。可以使用wait_for_server()方法来实现：
client.wait_for_server()
使用Action客户端发送导航目标点。创建一个MoveBaseGoal，并填充所需的信息，然后发送目标点：
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position.x = 1.0 # 设置目标点的x坐标
goal.target_pose.pose.position.y = 2.0 # 设置目标点的y坐标
client.send_goal(goal)
使用Action客户端可以监视导航状态。可以使用wait_for_result()方法来等待导航完成，并检查返回的结果来确定导航是否成功完成。完成的情况下状态码应该是3
client.wait_for_result()
result = client.get_result()
if result and result.status == 3: # status 3 表示导航成功完成
print("导航已完成")
else:
print("导航未成功完成")
2. Topic方式
可以通过订阅ROS话题的回调函数来监视导航状态。订阅move_base节点发布的/move_base/status话题，并在回调函数中处理导航状态。
这是一个更加直接的办法，回调函数可以直接获取状态码。
import rospy
from actionlib_msgs.msg import GoalStatusArray
def callback(data):
if len(data.status_list) > 0:
# 获取最新的导航状态
status = data.status_list[-1]
if status.status == 3: # status 3 表示导航成功完成
print("导航已完成")
def main():
rospy.init_node('navigation_status_listener')
rospy.Subscriber('/move_base/status', GoalStatusArray, callback)
rospy.spin()
if __name__ == '__main__':
main()
3. 验证
当我们使用Rviz发布目标点之前，status_list为空的list
查看状态码话题命令
rostopic echo /move_base/status
当我们使用工具或者程序发布目标点之后，可以看到状态码变成了1，1代表着导航进行中。
当到达目标点后，可见状态码变为了3
4. 状态码取值
/move_base/status话题消息的状态字段（status）可以有多种不同的取值，每个取值表示不同的导航状态。这些状态通常使用整数值来表示，其中一些常见的状态取值包括：
0：PENDING（等待） - 导航目标正在等待处理。
1：ACTIVE（激活） - 导航目标正在被执行。
2：PREEMPTED（中断） - 导航目标被中断。
3：SUCCEEDED（成功完成） - 导航目标成功完成。
4：ABORTED（中止） - 导航目标被中止。
5：REJECTED（拒绝） - 导航目标被拒绝。
6：PREEMPTING（正在中断） - 导航目标正在被中断。
7：RECALLING（正在召回） - 导航目标正在被召回。
8：RECALLED（已召回） - 导航目标已经被召回。
9：LOST（丢失） - 导航目标丢失。
这些状态值在ROS中用于表示导航操作的不同阶段和结果。可以根据你的应用程序需求来监视这些状态，以确定导航任务的进展和结果。