ros学习笔记2.Robot Localization

在 Rviz 中可视化定位

• LaserScan Display
• Map Display
• PoseArray Display

执行以下命令以启动 amcl 节点。我们需要运行此节点才能可视化位姿数组。

roslaunch husky_navigation amcl_demo.launch

rosrun rviz rviz

可视化位姿阵列（粒子云）

• 单击显示下的添加按钮并选择 PoseArray 显示。
• 在显示属性中，输入发布粒子云的主题名称（通常为 /particlecloud）。
• 要查看机器人的位置，您还可以选择添加 RobotModel 或 TF 显示。
• 固定坐标系（Fixed Frame）必须设置为 map 才能正确可视化粒子。

练习

a) 添加 LaserScan 和 Map 显示，以便通过 RViz 可视化机器人在房间中的位置。

b) 使用 2D Pose Estimate 工具，在 RViz 中为 Husky 机器人设置初始位置和方向。它不必精确，只需查看 Husky 在模拟中的位置和方向，并尝试在 RViz 中以类似的方式设置它。

c) 使用键盘 teleop 程序让机器人在房间内移动。

roslaunch husky_navigation_launch keyboard_teleop.launch

note 1：ROS 中的定位通过称为粒子的元素进行可视化

note 2：云的扩散表示定位系统对机器人位姿的不确定性。

note 3：随着机器人在环境中移动，由于额外的扫描数据，该云的大小应该会缩小，从而使 amcl 能够改进其对机器人位置和方向的估计。

Monte Carlo定位 (MCL)

由于机器人可能不会总是按照预期移动，因此它会对下一步的移动方向产生许多随机猜测。这些猜测称为粒子。每个粒子都包含对未来可能位姿的完整描述。当机器人观察其所处的环境（通过传感器读数）时，它会丢弃与这些读数不匹配的粒子，并生成更多接近那些看起来更可能的粒子的粒子。这样，最终，大多数粒子将汇聚在机器人最可能的位姿中。因此，您移动得越多，从传感器获得的数据就越多，因此定位将更加精确。这些粒子就是您在上一个练习中在 RViz 中看到的箭头。很神奇，对吧？

这被称为Monte Carlo定位 (MCL) 算法，或粒子滤波器定位.

The AMCL package 

AMCL（自适应蒙特卡罗定位）包提供了 amcl 节点，该节点使用 MCL 系统来跟踪在 2D 空间中移动的机器人的定位。此节点订阅激光数据、基于激光的地图和机器人的变换，并发布其在地图中的估计位置。启动时，amcl 节点根据提供的参数初始化其粒子过滤器。

所以，基本上，您在上一个练习中所做的是：

• 首先，您使用预配置的 amcl_demo.launch 文件启动了一个 amcl 节点。
• 其次，使用 2D Pose Estimate 工具（将该位姿发布到 /initialpose 主题）设置初始位姿。
• 然后，您开始在房间内移动机器人，amcl 节点开始读取发布到激光主题（/scan）、地图主题（/map）和变换主题（/tf）的数据，并将机器人所处的估计位姿发布到 /amcl_pose 和 /particlecloud 主题。
• 最后，通过 RViz，您访问了此节点发布到 /particlecloud 主题的数据，因此您能够将其可视化，这要归功于“箭头”云，它们指示机器人所处的最可能位置及其方向。

练习

a) 查看 husky_navigation 包中的 amcl_demo.launch 文件。您将看到它实际上正在调用另一个名为 amcl.launch 的启动文件。提示：您可以使用 rospack find [package_name] 命令显示系统中任何 ROS 包的路径。

<launch>

  <!-- Run the map server -->
  <arg name="map_file" default="$(find husky_navigation)/maps/my_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!--- Run AMCL -->
  <include file="$(find husky_navigation)/launch/amcl.launch" />

  <!--- Run Move Base -->
  <include file="$(find husky_navigation)/launch/move_base.launch" />

</launch>

b) 创建一个名为 my_amcl_launcher 的新包。在此包内，创建一个名为 launch 的新目录。在此目录中，创建一个名为 change_map.launch 的新文件。将 amcl.launch 文件的内容复制到此新文件中。

cd ~/catkin_ws/src

catkin_create_pkg my_amcl_launcher

cd my_amcl_launcher
mkdir launch
cd launch
touch change_map.launch

c) 在您刚刚创建的启动文件中，添加一个启动 map_server 节点的部分（如您刚刚在上面看到的），并提供不同的地图文件。

<launch>

  <!-- Run the map server -->
  <arg name="map_file" default="$(find husky_navigation)/maps/playpen_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!--- Run AMCL -->
  <include file="$(find husky_navigation)/launch/amcl.launch" />

  <!--- Run Move Base -->
  <include file="$(find husky_navigation)/launch/move_base.launch" />

</launch>

d) 打开 RViz 并查看现在发生了什么。

通过 Rviz 和 /particlecloud 主题可视化了定位。但是，现在您已经了解到，这不是 amcl 节点写入的唯一主题。

无需使用 /particlecloud 主题即可获取有关机器人估计位姿的信息。

note 1：请记住，为了能够可视化这些主题，必须启动 amcl 节点。

note 2：将机器人移过房间​​并检查姿势是否正确变化。

-n1: 表示只接收并显示 1 条消息后即停止。-n 是用来指定要接收的消息数量的选项，而 1 是该数量的值。

练习

创建一个service server，当调用时，该服务器返回机器人当时的位姿（位置和方向）。

a) 创建一个名为 get_pose 的新包。添加 rospy 作为依赖项。

cd ~/catkin_ws/src

catkin_create_pkg get_pose rospy std_msgs nav_msgs

b) 在此包内，创建一个名为 get_pose_service.py 的文件。在此文件中，编写Service Server的代码。

cd get_pose
mkdir scripts
touch scripts/get_pose_service.py
chmod +x scripts/get_pose_service.py

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose
from get_pose.srv import GetPose, GetPoseResponse

class GetPoseService:
    def __init__(self):
        self.current_pose = None
        # 订阅 /odom 话题
        rospy.Subscriber("/odom", Odometry, self.odom_callback)

    def odom_callback(self, msg):
        # 从 /odom 消息中获取位姿并保存
        self.current_pose = msg.pose.pose
        rospy.loginfo("Updated pose: %s", self.current_pose)

    def handle_get_pose(self, req):
        rospy.loginfo("Service /get_pose called")
        
        # 如果还没有接收到位姿，返回一个空的 Pose
        if self.current_pose is None:
            rospy.logwarn("No pose data available yet!")
            return GetPoseResponse(Pose())
        
        rospy.loginfo("Returning current pose: %s", self.current_pose)
        return GetPoseResponse(self.current_pose)

def get_pose_server():
    rospy.init_node('get_pose_server')

    # 创建 GetPoseService 实例
    get_pose_service = GetPoseService()

    # 定义服务及其处理函数
    service = rospy.Service('get_pose', GetPose, get_pose_service.handle_get_pose)
    rospy.loginfo("GetPose 服务已准备好提供机器人的位姿信息。")

    # 保持服务运行
    rospy.spin()

if __name__ == "__main__":
    get_pose_server()

cd ~/catkin_ws/src/get_pose/srv
touch GetPose.srv

 GetPose.srv文件

---
geometry_msgs/Pose pose

c) 创建launch文件以启动您的服务。

mkdir ~/catkin_ws/src/get_pose/launch
touch ~/catkin_ws/src/get_pose/launch/get_pose.launch

<launch>
  <node pkg="get_pose" type="get_pose_service.py" name="get_pose_service" output="screen"/>
</launch>

CMakelists和package.xml需要加上对应依赖

d) 使用终端调用您的服务。

#启动服务
roslaunch get_pose get_pose.launch

#调用服务
rosservice call /get_pose

e) 使用您的服务，从环境中的某个位置获取姿势数据：

硬件要求

配置对于在环境中正确定位机器人也非常重要。为了获得正确的机器人定位，我们需要满足 3 个基本要求：

• 提供良好的激光数据
• 提供良好的里程计数据
• 提供良好的基于​​激光的地图数据

变换

amcl 节点对机器人的变换有以下两个要求：

1. amcl 将接收到的激光扫描数据转换到里程计坐标系（~odom_frame_id）。因此，激光扫描数据发布的坐标系与里程计坐标系之间必须在 tf 树中有一条路径。
2. amcl 查找激光坐标系和底盘坐标系（~base_frame_id）之间的变换，并将其永久固定。因此，amcl 无法处理相对于底盘移动的激光。**

为 AMCL 节点创建启动文件 

正如您在映射章节中看到的，您还需要一个启动文件才能启动 amcl 节点。此节点也是高度可定制的，我们可以配置许多参数以提高其性能。这些参数可以在启动文件本身中设置，也可以在单独的参数文件（YAML 文件）中设置。您可以在此处查看此节点具有的所有参数的完整列表：http://wiki.ros.org/amcl

让我们来看看一些最重要的参数：

通用参数

• odom_model_type（默认值："diff"）：设置使用的里程计模型。可以是 "diff"（差分）、"omni"（全向）、"diff-corrected"（差分校正）或 "omni-corrected"（全向校正）。
• odom_frame_id（默认值："odom"）：表示与里程计相关联的坐标系。
• base_frame_id（默认值："base_link"）：表示与机器人底盘相关联的坐标系。
• global_frame_id（默认值："map"）：表示由定位系统发布的坐标系名称。
• use_map_topic（默认值：false）：指示节点是否从主题获取地图数据或从服务调用中获取。

滤波器参数

这些参数允许你配置粒子滤波器的工作方式。

• min_particles（默认值：100）：设置滤波器允许的最小粒子数。
• max_particles（默认值：5000）：设置滤波器允许的最大粒子数。
• kld_err（默认值：0.01）：设置真实分布和估计分布之间允许的最大误差。
• update_min_d（默认值：0.2）：设置机器人必须移动的线性距离（以米为单位），以便进行滤波器更新。
• update_min_a（默认值：π/6.0）：设置机器人必须移动的角度距离（以弧度为单位），以便进行滤波器更新。
• resample_interval（默认值：2）：设置在重新采样之前所需的滤波器更新次数。
• transform_tolerance（默认值：0.1）：发布变换的时间（以秒为单位），以表明该变换在未来仍然有效。
• gui_publish_rate（默认值：-1.0）：用于可视化的扫描和路径发布的最大速率（以赫兹为单位）。如果该值为 -1.0，则该功能被禁用。

练习

a) 在之前练习创建的包中，创建一个名为 my_amcl_launch.launch 的新启动文件。将 amcl.launch 文件的内容复制到此文件。

b) 修改 min_particles 和 max_particles 参数。分别将它们设置为 1 和 5。

<?xml version="1.0"?>

<launch>

  <arg name="use_map_topic" default="true"/> <!-- 是否使用地图主题 -->
  <arg name="scan_topic" default="scan" /> <!-- 激光扫描数据主题 -->

  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic" value="$(arg use_map_topic)"/> <!-- 使用地图主题的参数 -->
    <!-- 从最佳位置发布扫描数据，最大频率为 10 Hz -->
    <param name="odom_model_type" value="diff"/> <!-- 里程计模型类型 -->
    <param name="odom_alpha5" value="0.1"/> <!-- 里程计模型参数 α5 -->
    <param name="gui_publish_rate" value="10.0"/> <!-- 可视化发布速率 -->
    <param name="laser_max_beams" value="60"/> <!-- 激光最大光束数 -->
    <param name="laser_max_range" value="12.0"/> <!-- 激光最大测量范围 -->
    <param name="min_particles" value="1"/> <!-- 粒子滤波器最小粒子数 -->
    <param name="max_particles" value="5"/> <!-- 粒子滤波器最大粒子数 -->
    <param name="kld_err" value="0.05"/> <!-- KLD 误差 -->
    <param name="kld_z" value="0.99"/> <!-- KLD 置信度 -->
    <param name="odom_alpha1" value="0.2"/> <!-- 里程计模型参数 α1 -->
    <param name="odom_alpha2" value="0.2"/> <!-- 里程计模型参数 α2 -->
    <!-- 平移标准差，单位：米 -->
    <param name="odom_alpha3" value="0.2"/> <!-- 里程计模型参数 α3 -->
    <param name="odom_alpha4" value="0.2"/> <!-- 里程计模型参数 α4 -->
    <param name="laser_z_hit" value="0.5"/> <!-- 激光测距命中概率 -->
    <param name="laser_z_short" value="0.05"/> <!-- 激光测距短距离概率 -->
    <param name="laser_z_max" value="0.05"/> <!-- 激光测距最大距离概率 -->
    <param name="laser_z_rand" value="0.5"/> <!-- 激光测距随机概率 -->
    <param name="laser_sigma_hit" value="0.2"/> <!-- 激光测距命中标准差 -->
    <param name="laser_lambda_short" value="0.1"/> <!-- 激光测距短距离衰减因子 -->
    <param name="laser_model_type" value="likelihood_field"/> <!-- 激光模型类型 -->
    <!-- <param name="laser_model_type" value="beam"/> --> <!-- 激光模型类型（备用） -->
    <param name="laser_likelihood_max_dist" value="2.0"/> <!-- 激光模型最大有效距离 -->
    <param name="update_min_d" value="0.25"/> <!-- 进行滤波器更新所需的最小线性距离 -->
    <param name="update_min_a" value="0.2"/> <!-- 进行滤波器更新所需的最小角度距离 -->
    <param name="odom_frame_id" value="odom"/> <!-- 里程计坐标系 -->
    <param name="resample_interval" value="1"/> <!-- 重新采样前所需的滤波器更新次数 -->
    <!-- 增加容差，因为计算机可能会非常忙碌 -->
    <param name="transform_tolerance" value="1.0"/> <!-- 变换容差时间 -->
    <param name="recovery_alpha_slow" value="0.0"/> <!-- 慢速恢复参数 -->
    <param name="recovery_alpha_fast" value="0.0"/> <!-- 快速恢复参数 -->
    <remap from="scan" to="$(arg scan_topic)"/> <!-- 重新映射扫描主题 -->
  </node>

</launch>

c) 使用这个新的启动文件再次启动节点，看看会发生什么。

用于定位机器人的粒子（绿色箭头）数量急剧减少。这意味着定位算法能够进行的猜测次数（关于机器人的姿势）将非常缓慢。这将导致机器人的定位更加不精确。

激光参数

这些参数将允许你配置 amcl 节点与激光的交互方式。

• laser_min_range（默认值：-1.0）：要考虑的最小扫描范围；-1.0 将使用激光报告的最小范围。
• laser_max_range（默认值：-1.0）：要考虑的最大扫描范围；-1.0 将使用激光报告的最大范围。
• laser_max_beams（默认值：30）：每次扫描中用于更新滤波器的均匀分布的光束数量。
• laser_z_hit（默认值：0.95）：模型中 z_hit 部分的混合权重。
• laser_z_short（默认值：0.1）：模型中 z_short 部分的混合权重。
• laser_z_max（默认值：0.05）：模型中 z_max 部分的混合权重。
• laser_z_rand（默认值：0.05）：模型中 z_rand 部分的混合权重。

练习

a) 修改 laser_max_range 参数，并将其设置为 1。

b) 再次启动节点，看看会发生什么。

AMCL的服务 

到目前为止，我们已经了解了如何通过主题进行定位。我们从激光、里程计和地图主题中获取数据，并将这些数据发布到两个新的主题中，以提供机器人的定位信息。然而，如果需要的话，也可以涉及一些服务。让我们快速了解一下：

amcl 节点提供的服务

• global_localization (std_srvs/Empty)：启动全局定位，其中所有粒子在地图的自由空间中随机分散。

amcl 节点调用的服务

• static_map (nav_msgs/GetMap)：amcl 调用此服务以检索用于基于激光的定位的地图。

总结一下，amcl 节点提供一个名为 global_localization 的服务，用于重新启动粒子的位置，并且能够使用一个名为 static_map 的服务来获取所需的地图数据。

练习

a) 启动 RViz 以可视化 /particlecloud 主题。

source ~/catkin_ws/devel/setup.bash
roslaunch husky_navigation amcl_demo.launch
rosrun rviz rviz

b) 创建一个名为 initialize_particles 的新包。添加 rospy 作为依赖项。

cd ~/catkin_ws/src
catkin_create_pkg initialize_particles rospy
cd ~/catkin_ws
catkin_make
cd ~/catkin_ws/src/initialize_particles
mkdir scripts
chmod +x scripts
touch scripts/init_particles_caller.py
chmod +x scripts/init_particles_caller.py

c) 在此包内，创建一个名为 init_particles_caller.py 的新文件。在此 Python 文件中，编写服务客户端的代码。此客户端将调用 /global_localization 服务以分散粒子。

import rospy
from std_srvs.srv import Empty

def call_global_localization():
    # 初始化 ROS 节点
    rospy.init_node('init_particles_client')
    
    # 等待 global_localization 服务变得可用
    rospy.wait_for_service('/global_localization')
    
    try:
        # 创建 /global_localization 服务的客户端
        global_localization = rospy.ServiceProxy('/global_localization', Empty)
        
        # 调用服务
        response = global_localization()
        rospy.loginfo("粒子已初始化。")
    except rospy.ServiceException as e:
        rospy.logerr("服务调用失败: %s" % e)

if __name__ == "__main__":
    try:
        call_global_localization()
    except rospy.ROSInterruptException:
        pass

 所以我有一个服务，它允许我在整个环境中随机分散过滤粒子。好吧，但是......我为什么需要这个？它对我真的很有用吗？

这实际上是一个很好的问题。答案是肯定的！当然！到目前为止，在启动​​ amcl 节点时，您正在使用 RViz（2D 姿势估计工具）来提供机器人的近似姿势。这样，您就为 amcl 节点提供了巨大的帮助，以便定位机器人。这就是为什么所有粒子都初始化为接近机器人的位置。

但让我问你......如果你不知道机器人实际上在哪里会发生什么？或者如果你不能使用 RViz，那么你无法提供机器人的近似姿势会发生什么？

如果是这种情况，那么您将不得不初始化整个环境中的粒子，并开始在其周围移动机器人。这样，传感器将开始收集有关环境的信息并将其提供给机器人。而且正如您所知，随着机器人不断获得越来越多的环境信息，过滤器的粒子将变得越来越准确。

让我们做一个练习，以便您更好地理解它的工作原理。

练习

a) 在您在上面的练习中创建的包中，创建一个名为 square_move.py 的新文件。

cd ~/catkin_ws/src/initialize_particles
touch scripts/square_move.py
chmod +x scripts/square_move.py

b) 在此文件中，编写代码以移动机器人。机器人必须执行方形运动。完成后，它必须将过滤粒子的协方差打印到屏幕上。

#!/usr/bin/env python

import rospy
import time
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped  # 更新为从 /amcl_pose 订阅消息
from math import sqrt

# 全局变量，用于存储粒子协方差
particle_covariance = None

# 机器人方形运动函数
def move_square():
    rospy.init_node('square_move_node')

    # 创建一个发布器，用于向 /cmd_vel 话题发送速度指令
    velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    # 定义运动参数
    square_side_length = 2.0  # 方形边长（米）
    linear_speed = 0.2  # 线速度（米/秒）
    angular_speed = 0.5  # 角速度（弧度/秒）
    rate = rospy.Rate(10)

    move_cmd = Twist()
    
    # 执行方形运动
    for _ in range(4):
        # 直线移动
        move_cmd.linear.x = linear_speed
        move_cmd.angular.z = 0
        start_time = time.time()
        while time.time() - start_time < square_side_length / linear_speed:
            velocity_publisher.publish(move_cmd)
            rate.sleep()

        # 停止前进，准备转弯
        move_cmd.linear.x = 0
        velocity_publisher.publish(move_cmd)
        time.sleep(1)

        # 转弯90度
        move_cmd.angular.z = angular_speed
        start_time = time.time()
        while time.time() - start_time < 3.14 / 2 / angular_speed:
            velocity_publisher.publish(move_cmd)
            rate.sleep()

        # 停止转弯
        move_cmd.angular.z = 0
        velocity_publisher.publish(move_cmd)
        time.sleep(1)

    # 运动结束后打印粒子协方差
    print("粒子滤波器协方差: ", particle_covariance)

# 回调函数，从 /amcl_pose 获取粒子协方差
def amcl_pose_callback(msg):
    global particle_covariance
    # 提取协方差矩阵中的 x, y 和 z 方向的协方差
    covariance_matrix = msg.pose.covariance
    covariance_x = covariance_matrix[0]   # 协方差矩阵中的第一个值 (x)
    covariance_y = covariance_matrix[7]   # 协方差矩阵中的第八个值 (y)
    covariance_z = covariance_matrix[35]  # 协方差矩阵中的最后一个值 (z)

    # 计算粒子协方差的总和
    particle_covariance = sqrt(covariance_x + covariance_y + covariance_z)

if __name__ == '__main__':
    try:
        # 订阅 /amcl_pose 话题，获取协方差信息
        rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_pose_callback)

        move_square()
    except rospy.ROSInterruptException:
        pass

c) 测试您的代码是否有效（机器人在方形中移动）。

roslaunch husky_navigation amcl_demo.launch
rosrun initialize_particles square_move.py

d) 测试它是否有效后，添加必要的代码，以便您的程序执行以下操作：

首先，它调用必要的服务以将粒子分散到整个环境中。

当粒子随机分散时，它会执行方形运动。

当它完成移动后，它会检查粒子的协方差。

如果这个协方差小于 0.65，则意味着机器人已正确定位自身。然后，程序将结束。如果这个协方差大于 0.65，它将重复整个过程（分散粒子，执行移动，检查协方差……）。

现在，让我们扩展程序，使其分散粒子，检查协方差，并重复该过程，直到协方差小于 0.65。

• 添加使用 /global_localization 服务分散粒子的逻辑。
• 添加检查协方差和重复该过程的逻辑。

完整代码：

#!/usr/bin/env python

import rospy
import time
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped  # 更新为从 /amcl_pose 订阅消息
from std_srvs.srv import Empty
from math import sqrt

# 全局变量，用于存储粒子协方差
particle_covariance = None

# 分散粒子
def disperse_particles():
    rospy.wait_for_service('/global_localization')
    try:
        global_localization = rospy.ServiceProxy('/global_localization', Empty)
        global_localization()
        rospy.loginfo("成功分散粒子。")
    except rospy.ServiceException as e:
        rospy.logerr("分散粒子失败: %s" % e)

# 机器人方形运动函数
def move_square():
    velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    # 定义运动参数
    square_side_length = 2.0  # 方形边长（米）
    linear_speed = 0.2  # 线速度（米/秒）
    angular_speed = 0.5  # 角速度（弧度/秒）
    rate = rospy.Rate(10)

    move_cmd = Twist()
    
    # 执行方形运动
    for _ in range(4):
        # 直线移动
        move_cmd.linear.x = linear_speed
        move_cmd.angular.z = 0
        start_time = time.time()
        while time.time() - start_time < square_side_length / linear_speed:
            velocity_publisher.publish(move_cmd)
            rate.sleep()

        # 停止前进，准备转弯
        move_cmd.linear.x = 0
        velocity_publisher.publish(move_cmd)
        time.sleep(1)

        # 转弯90度
        move_cmd.angular.z = angular_speed
        start_time = time.time()
        while time.time() - start_time < 3.14 / 2 / angular_speed:
            velocity_publisher.publish(move_cmd)
            rate.sleep()

        # 停止转弯
        move_cmd.angular.z = 0
        velocity_publisher.publish(move_cmd)
        time.sleep(1)

    rospy.loginfo("完成方形运动")

# 回调函数，从 /amcl_pose 获取粒子协方差
def amcl_pose_callback(msg):
    global particle_covariance
    # 提取协方差矩阵中的 x, y 和 z 方向的协方差
    covariance_matrix = msg.pose.covariance
    covariance_x = covariance_matrix[0]   # 协方差矩阵中的第一个值 (x)
    covariance_y = covariance_matrix[7]   # 协方差矩阵中的第八个值 (y)
    covariance_z = covariance_matrix[35]  # 协方差矩阵中的最后一个值 (z)

    # 计算粒子协方差的总和
    particle_covariance = sqrt(covariance_x + covariance_y + covariance_z)

if __name__ == '__main__':
    try:
        # 初始化节点
        rospy.init_node('square_move_node')

        # 订阅 /amcl_pose 话题，获取协方差信息
        rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_pose_callback)

        while True:
            # 步骤1：分散粒子
            disperse_particles()

            # 步骤2：执行方形运动
            move_square()

            # 步骤3：检查粒子协方差
            rospy.sleep(1)  # 等待协方差更新
            if particle_covariance is not None:
                rospy.loginfo("当前协方差: %f" % particle_covariance)
                if particle_covariance < 0.65:
                    rospy.loginfo("机器人已成功定位！")
                    break
                else:
                    rospy.loginfo("协方差过大，重新执行该过程...")

    except rospy.ROSInterruptException:
        pass

 总结

为了自主地在地图上导航，机器人需要能够将自己定位到地图中。而这正是 amcl 节点（amcl 包）为我们提供的功能。为了实现这一点，amcl 节点使用 MCL（蒙特卡罗定位）算法。

正如您在上一章中学到的，没有地图就无法导航。但是，如果您的机器人无法在地图中定位自己，您当然也无法导航。因此，定位过程是 ROS 导航的另一个关键部分。

基本上，amcl 节点从机器人的激光和里程计以及环境地图获取数据，并输出机器人的估计姿势。机器人在环境中移动得越多，定位系统获得的数据就越多，因此它返回的估计姿势就越精确。