#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped
class MultiPointNavigator:
def __init__(self):
# 创建一个节点，并且在节点后面自动随机生成一个数，确保节点名字的唯一性
rospy.init_node('multi_point_navigator', anonymous=True)
# `SimpleActionClient`对象去连接到一个名为`move_base`的动作服务器。`move_base`服务器负责接收导航目标点，计算到达该目标点的路径，并驱动机器人沿着计算出的路径移动。
# `MoveBaseAction`：这是一个动作定义，它规定了客户端和`move_base`动作服务器之间交换的目标、反馈和结果的消息类型
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 没有设置时间，无限期地等待，直到与服务器的连接建立
self.client.wait_for_server()
rospy.loginfo("Connected to move_base server")
def move_to_goal(self, pose):
# `MoveBaseGoal`是一个特定的消息类型，用于与`move_base`动作服务器进行通信
goal = MoveBaseGoal()
# 设置目标位置使用的坐标系
goal.target_pose.header.frame_id = "map"
# 为目标位置的时间戳赋值，使用当前时间
goal.target_pose.header.stamp = rospy.Time.now()
# 位姿信息，由下文传入
goal.target_pose.pose = pose
rospy.loginfo("Sending goal")
# 发送一个动作目标给动作服务器
self.client.send_goal(goal)
# 使得动作客户端等待动作完成
self.client.wait_for_result()
# 如果动作目标已成功完成。服务器已完成任务，并且按照预期达到了目标状态，则等式成立
if self.client.get_state() == actionlib.GoalStatus.SUCCEEDED:
rospy.loginfo("Goal reached")
return True
else:
rospy.loginfo("Failed to reach the goal")
return False
def navigate_to_points(self, points):
for pose in points:
if not self.move_to_goal(pose):
rospy.loginfo("Stop navigating due to failure in reaching a point")
break
if __name__ == '__main__':
try:
navigator = MultiPointNavigator()
points = [
# Add your points here. Example:
# PoseStamped().pose.position.x = 1.0
# PoseStamped().pose.position.y = 1.0
# PoseStamped().pose.orientation.w = 1.0
# And so on for each point
]
navigator.navigate_to_points(points)
except rospy.ROSInterruptException:
pass
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped 这行代码在ROS（Robot Operating System）环境下使用，它从geometry_msgs包中导入了PoseStamped消息类型。geometry_msgs包是ROS中用于表示位置和方向的标准消息包之一。
PoseStamped详解
PoseStamped消息类型用于表示一个带时间戳的位姿（位置和方向）。它广泛用于ROS中，尤其是在导航、定位和机器人运动规划等领域。PoseStamped包含两个主要部分：header和pose。
-
Header: 包含时间戳和坐标帧信息。时间戳（
stamp）用于记录消息的确切时间，而坐标帧（frame_id）指定了位姿数据相对于哪个参考坐标系。这对于确保数据的一致性和正确的坐标变换非常重要。 -
Pose: 包含实际的位置（
position）和方向（orientation）信息。- Position: 由
x、y和z坐标组成，表示空间中的一个点。 - Orientation: 通常由四元数（
x、y、z、w）表示，用于定义物体的方向。
- Position: 由
使用示例
在ROS中，PoseStamped消息常用于指定机器人或其他物体的目标位置和朝向。例如，在发送导航目标给move_base节点时，就会使用到PoseStamped消息。这样，move_base节点就能根据给定的位置和方向信息规划路径并移动机器人。
goal = PoseStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "map"
goal.pose.position.x = 1.0
goal.pose.position.y = 2.0
goal.pose.position.z = 0.0
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = 0.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 1.0
在这个例子中，我们创建了一个PoseStamped消息实例goal，指定了它的时间戳、坐标帧、位置和方向。这样的消息可以用于告诉ROS系统我们希望机器人移动到某个具体的空间位置，面向某个特定的方向。
self.client.wait_for_result()
- 此行代码使得动作客户端等待动作完成。这意味着在动作服务器完成其任务（例如，成功导航到目标位置或者在尝试达到目标时遇到错误导致失败）之前，这行代码会阻塞程序的进一步执行。
wait_for_result方法的调用是同步的，意味着它将会在动作完成或失败后才返回。这对于确保任务按顺序执行（比如，在继续执行程序的下一部分之前确保机器人已经到达指定位置）非常有用。- 如果动作成功完成，动作服务器将发送一个结果消息，客户端可以通过调用
get_result方法来检索这个结果。如果动作失败，根据动作服务器的实现，还可以获取失败的具体原因。
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal详解
在ROS（Robot Operating System）中，move_base_msgs包是专门用于移动基础设施（例如机器人导航）的一部分。它提供了一系列消息和服务类型，使得可以通过move_base节点与导航功能进行交互。其中，MoveBaseAction和MoveBaseGoal是实现机器人导航任务时非常关键的两个组件。下面分别对这两个组件进行详解：
MoveBaseAction
MoveBaseAction定义了一个动作（action），它是actionlib库使用的一个高级通信方式，用于执行可能需要较长时间完成的任务。动作通信机制包括目标（Goal）、反馈（Feedback）和结果（Result）消息类型。对于MoveBaseAction来说，这意味着：
- Goal：你要发送给
move_base节点的导航目标，通常是一个位姿（位置和朝向），表明机器人需要移动到的目标位置。 - Feedback：在机器人移动过程中，
move_base节点会周期性地发送当前的状态和位置作为反馈，这可以帮助监控任务的进展。 - Result：一旦机器人到达目标位置或者导航任务以某种方式终止（例如失败或被取消），
move_base节点会发送一个结果消息，表明导航任务的最终状态。
MoveBaseGoal
MoveBaseGoal是MoveBaseAction动作的目标部分的具体实现。它定义了发送给move_base动作服务器的导航目标的具体数据结构。MoveBaseGoal通常包含以下信息：
- target_pose：这是一个
PoseStamped消息，指定了机器人的目标位置和朝向。PoseStamped包括了位置（使用x、y、z坐标）和方向（使用四元数表示），同时还包含了这个位姿相对于哪个坐标系（通过frame_id）以及时间戳（stamp），确保目标位置在正确的空间和时间上下文中被解释。
在实际应用中，当你想让机器人自主导航到某个位置时，你会创建一个MoveBaseGoal实例，设置其中的target_pose为你希望机器人到达的位置和朝向，然后通过SimpleActionClient发送这个目标给move_base动作服务器。move_base节点随后开始处理这个导航任务，尝试规划路径并控制机器人移动到目标位置。
简而言之，MoveBaseAction和MoveBaseGoal在ROS中用于实现机器人的自主导航功能，通过它们可以定义导航任务、发送导航目标，以及接收任务进展反馈和结果。