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中用于表示导航操作的不同阶段和结果。可以根据你的应用程序需求来监视这些状态，以确定导航任务的进展和结果。