ROS-SLAM-hectormapping-launch启动hector-Hector参数-TF-里程计-Gmapping-launch启动gmapping-gmapping参数设置-保存加载地图

SLAM

SLAM（Simultaneous Localization and Mapping，同步定位与建图）是机器人和无人驾驶领域的核心技术之一。它通过传感器数据（如激光雷达、摄像头等）实时估计机器人或无人系统的位姿（位置和姿态），并构建环境地图。

hector_mapping

hector_mapping 是一个用于基于激光雷达（LIDAR）的 SLAM（Simultaneous Localization and Mapping，同时定位与建图）的节点，适用于没有里程计（odometry）且计算资源有限的场景。

• 订阅主题
  scan (sensor_msgs/LaserScan)
  用于 SLAM 系统的激光雷达扫描数据。
  syscommand (std_msgs/String)
  系统命令。如果字符串等于 “reset”，则地图和机器人姿态将重置为初始状态。
• 发布主题
  map_metadata (nav_msgs/MapMetaData)
  从该主题可以获取地图数据，数据是持久化的，并定期更新。
  map (nav_msgs/OccupancyGrid)
  从该主题可以获取地图数据，数据是持久化的，并定期更新。
  slam_out_pose (geometry_msgs/PoseStamped)
  估计的机器人姿态，不包含协方差。
  poseupdate (geometry_msgs/PoseWithCovarianceStamped)
  估计的机器人姿态，包含高斯估计的不确定性。
  

sudo apt install ros-noetic-hector-mapping

rosrun hector_mapping  hector_mapping 

rosrun rviz rviz

然后add robotmodule+map+laserscan，再分别订阅话题

最后控制机器人移动来SLAM建图

 rosrun rqt_robot_steering rqt_robot_steering 

launch启动hector

catkin_create_pkg  slam_pkg roscpp rospy std_msgs

新建一个launch文件夹，里面存放launch文件

$(find wpr_simulation)相当于执行rospack find wpr_simulation 指令的结果

<launch>

<include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" >

</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
<node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
</launch>

Hector_Mapping参数设置

1. ~map_update_distance_thresh
   类型：double（浮点数）
   默认值：0.4
   单位：米（m）
   作用：
   这是地图更新的距离阈值。机器人需要移动至少 0.4 米（默认值）的距离，才会触发一次地图更新。
   详细说明：
   如果机器人移动的距离小于这个阈值，地图不会更新。
   这个参数与 ~map_update_angle_thresh 配合使用，满足其中一个条件即可触发地图更新。

2. ~map_update_angle_thresh
   类型：double（浮点数）
   默认值：0.9
   单位：弧度（rad）
   作用：
   这是地图更新的角度阈值。机器人需要旋转至少 0.9 弧度（默认值），才会触发一次地图更新。
   详细说明：
   如果机器人旋转的角度小于这个阈值，地图不会更新。
   这个参数与 ~map_update_distance_thresh 配合使用，满足其中一个条件即可触发地图更新。

3. ~map_pub_period
   类型：double（浮点数）
   默认值：2.0
   单位：秒（s）
   作用：
   这是地图发布的时间间隔。每隔 2.0 秒（默认值），地图数据会被发布一次。
   详细说明：
   即使机器人没有移动或旋转，地图也会按照这个时间间隔定期发布。
   这个参数与 ~map_update_distance_thresh 和 ~map_update_angle_thresh 无关，它只是控制地图发布的频率。

<launch>

<include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch" />
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" >
    <param name="map_update_distance_thresh"  value="0.1" />
    <param name="map_update_angle_thresh"  value="0.1" />
    <param name="map_update_distance_thresh"  value="0.1" />
</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz" />
<node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" />
</launch>

ROS的TF系统（定位）

ROS（Robot Operating System）中的 TF（Transform）系统 是一个用于管理坐标系变换的核心工具。它允许用户在机器人系统中定义和查询不同坐标系之间的关系，例如机器人底盘、传感器、机械臂等之间的相对位置和方向。TF 系统在机器人导航、感知和控制中起着至关重要的作用。

---

1. TF 系统的基本概念
   1.1 坐标系（Frame）

• 在机器人系统中，每个部件（如传感器、机械臂、底盘等）都有自己的坐标系。

• 坐标系通常用 父子关系 表示。例如，激光雷达的坐标系可能是机器人底盘坐标系的子坐标系。

  1.2 变换（Transform）

• 变换 描述了从一个坐标系到另一个坐标系的转换关系，包括平移（位置）和旋转（方向）。

• 变换可以用 4x4 的变换矩阵 或 平移向量 + 四元数（Quaternion） 表示。

  1.3 时间戳（Timestamp）

• 变换是随时间变化的（例如移动的机器人），因此每个变换都有一个时间戳，表示该变换的有效时间。

---

2. TF 系统的核心功能
   2.1 发布变换（Publish Transform）

• 通过 tf::TransformBroadcaster 发布坐标系之间的变换关系。
• 例如，发布机器人底盘到激光雷达的变换。

2.2 查询变换（Lookup Transform）

• 通过 tf::TransformListener 查询两个坐标系之间的变换关系。
• 例如，查询激光雷达坐标系到地图坐标系的变换。

2.3 管理变换树（Transform Tree）

• TF 系统维护一个 变换树，其中每个节点是一个坐标系，边是坐标系之间的变换关系。
• 变换树必须是一个有向无环图（DAG），即不能有循环依赖。

---

3. TF 系统的使用
   3.1 发布变换
   以下是一个发布变换的示例代码：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle nh;

    tf::TransformBroadcaster broadcaster;

    ros::Rate rate(10); // 10 Hz
    while (ros::ok()) {
        // 定义变换：从 "base_link" 到 "laser"
        tf::Transform transform;
        transform.setOrigin(tf::Vector3(0.1, 0.0, 0.2)); // 平移 (x, y, z)
        transform.setRotation(tf::Quaternion(0, 0, 0, 1)); // 旋转 (x, y, z, w)

        // 发布变换
        broadcaster.sendTransform(
            tf::StampedTransform(transform, ros::Time::now(), "base_link", "laser"));

        rate.sleep();
    }

    return 0;
}

模拟

• 发布从 base_link（机器人底盘）到 laser（激光雷达）的变换。
• 变换包括平移 (0.1, 0.0, 0.2) 和旋转 (0, 0, 0, 1)（无旋转）。

3.2 查询变换
以下是一个查询变换的示例代码：

#include <ros/ros.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle nh;

    tf::TransformListener listener;

    ros::Rate rate(10); // 10 Hz
    while (ros::ok()) {
        tf::StampedTransform transform;
        try {
            // 查询从 "base_link" 到 "laser" 的变换
            listener.lookupTransform("base_link", "laser", ros::Time(0), transform);

            // 打印变换信息
            ROS_INFO("Translation: [%f, %f, %f]",
                     transform.getOrigin().x(),
                     transform.getOrigin().y(),
                     transform.getOrigin().z());
            ROS_INFO("Rotation: [%f, %f, %f, %f]",
                     transform.getRotation().x(),
                     transform.getRotation().y(),
                     transform.getRotation().z(),
                     transform.getRotation().w());
        } catch (tf::TransformException &ex) {
            ROS_ERROR("%s", ex.what());
        }

        rate.sleep();
    }

    return 0;
}

• 解释：
  • 查询从 base_link 到 laser 的变换。
  • 打印变换的平移和旋转信息。

---

4. TF 系统的常用工具
   4.1 tf_monitor

• 查看当前 TF 树的状态。
• 命令：

  rosrun tf tf_monitor
  

4.2 view_frames

• 生成 TF 树的图形化表示（PDF 文件）。
• 命令：

  rosrun tf view_frames
  

4.3 tf_echo

• 实时查看两个坐标系之间的变换。
• 命令：

  rosrun tf tf_echo base_link laser
  

---

roslaunch wpr_simulation wpb_hector.launch 

rostopic list

llk@LLK:~/AI_Study_Note/Embodied-AI/ROS/catkin_ws/src$ rostopic type /tf
tf2_msgs/TFMessage

https://docs.ros.org/en/api/tf2_msgs/html/msg/TFMessage.html

tf2_msgs/TFMessage 是一个消息类型，用于在 ROS（Robot Operating System）中传递多个坐标变换信息。
transforms：这是一个数组，包含多个 geometry_msgs/TransformStamped 消息。每个 TransformStamped 消息表示一个坐标系之间的变换关系。
用途：TFMessage 通常用于在 ROS 节点之间传递多个坐标变换数据。例如，一个节点可以发布一个 TFMessage 消息，包含多个坐标系之间的变换关系，其他节点可以订阅该消息以获取这些变换信息。

geometry_msgs/TransformStamped 是一个消息类型，用于表示一个带时间戳的坐标变换。

1. header：这是一个 std_msgs/Header 消息，包含时间戳和坐标系信息。
   header.stamp：时间戳，表示变换的时间。
   header.frame_id：父坐标系的名称。
2. child_frame_id：子坐标系的名称，表示该变换是从父坐标系到子坐标系的变换。
3. transform：这是一个 geometry_msgs/Transform 消息，包含平移（translation）和旋转（rotation）信息。
   transform.translation：平移向量，包含 x、y、z 三个分量。 就是子坐标系原点在父坐标系的位置
   transform.rotation：旋转四元数，包含 x、y、z、w 四个分量。子坐标系的x，y，z轴的旋转变化或者表示该物体相对另一个坐标系的旋转角度和方向。

rostopic echo /tf

rosrurqt_tf_tree  rqt_tf_tree 

什么是里程计

电机里程计在激光雷达 SLAM（Simultaneous Localization and Mapping，同时定位与建图）中起着重要的作用。它通过记录电机的转动数据来计算机器人的位移信息，从而推算出机器人的当前位置。

这种位移信息以 TF 消息包的形式发布到话题 /tf 中。电机里程计的输出是 odom 到 base_footprint 的 TF，而激光雷达 SLAM 输出的是 map 到 base_footprint 的 TF。

电机里程计的优势在于，它不依赖于外部参照物的特征，因此不会受到参照物特征的误导。

然而，电机里程计存在误差，例如机器人轮胎打滑时，里程计的定位信息可能会与实际位置不相符。此外，这种误差会不断累积，使得定位误差越来越大。因此，需要使用障碍物点云配准算法来修正里程计的误差

roslaunch wpr_simulation wpb_corridor_hector.launch 

Hector Mapping 是一种基于激光雷达的 SLAM 算法，其核心思想是通过雷达点云和障碍物配准的方法来进行定位。在 Hector Mapping 中，机器人的位移主要由雷达点云配准算法计算得出，而不使用里程计的数据。为了在 RViz 中显示地图和机器人模型，Hector Mapping 会输出一段 map 到 odom 的 TF，以抵消不断增长的里程计 TF，从而让 base_footprint 的位置始终与 scanmatcher_frame 保持一致。

在运行 Hector Mapping 时，如果机器人进入两侧墙体平滑的走廊，激光雷达只能扫描到两侧的墙壁，导致障碍物点云配准的结果认为机器人没有往前移动，误以为是雷达数据的噪声导致定位在一个小范围内抖动。此时，Hector Mapping 会输出一段与机器人运动方向相反的 TF，以抵消里程计的变化，让 base_footprint 的位置始终与 scanmatcher_frame 保持一致。

直接将雷达点云匹配得到的位移是scanmatcher_frame

roslaunch wpr_simulation wpb_corridor_hector.launch 

Gmapping 是另一种常用的 SLAM 算法，其核心思想是先使用里程计推算机器人的位移，然后通过雷达点云贴合障碍物轮廓来修正里程计的误差。在 Gmapping 中，机器人的位移主要由里程计推算，激光雷达点云配准算法只是为了修正里程计的误差。Gmapping 的输出是 map 到 odom 再到 base_footprint 的 TF。

在运行 Gmapping 时，当机器人进入两侧墙体平滑的走廊，map 和 odom 的位置开始分离，并且出现了明显的跳动。这是由于激光雷达点云和障碍物配准导致的定位跳动。尽管如此，由于里程计还在运转，机器人的定位还是在继续推进的，最终的建图结果和实际场地基本保持一致。

map到odom的位置是障碍物点云匹配的偏移
odom到base_footprint是里程计的偏移

Gmapping

roslaunch wpr_simulation wpb_stage_robocup.launch 

rosrun gmapping  slam_gmapping

slam_gmapping 是一个 ROS（Robot Operating System）节点，用于同时定位与建图（SLAM）。它接收激光扫描数据（sensor_msgs/LaserScan）并构建地图（nav_msgs/OccupancyGrid）。地图可以通过 ROS 主题或服务获取。

• 订阅主题
  tf (tf/tfMessage)
  作用：提供激光、基座和里程计之间的坐标变换。
  说明：这些变换是构建地图和定位机器人所必需的。
  scan (sensor_msgs/LaserScan)
  作用：接收激光扫描数据，用于创建地图。
  说明：激光扫描数据是构建地图的主要输入数据。
• 发布主题
  map_metadata (nav_msgs/MapMetaData)
  作用：获取地图元数据。
  说明：该主题发布地图的元数据，包括地图的分辨率、尺寸等信息。数据会被缓存并定期更新。
  map (nav_msgs/OccupancyGrid)
  作用：获取地图数据。
  说明：该主题发布地图数据，表示为占用网格（Occupancy Grid）。数据会被缓存并定期更新。
  ~entropy (std_msgs/Float64)
  作用：估计机器人位姿分布的熵。
  说明：熵值越高表示机器人位姿的不确定性越大。这是从 1.1.0 版本开始新增的功能。
• 所需的坐标变换
  <传入扫描的坐标系> → base_link
  作用：将激光扫描数据从激光坐标系转换到机器人基座坐标系。
  说明：这通常是一个固定的变换，由 robot_state_publisher 或 tf static_transform_publisher 定期广播。
  base_link → odom
  作用：将机器人基座坐标系与里程计坐标系关联。
  说明：这通常由里程计系统提供，例如移动基座的驱动程序。
• 提供的坐标变换
  map → odom
  作用：提供机器人在地图坐标系中的当前位姿估计。
  说明：该变换表示机器人在地图中的位置和姿态。

roslaunch wpr_simulation wpb_stage_robocup.launch 
rosrun rqt_tf_tree  rqt_tf_tree 
rosrun gmapping  slam_gmapping 
rviz
rosrun wpr_simulation  keyboard_vel_ctrl 

launch 启动gmapping

<launch>

<include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"   />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz" />
<node pkg="wpr_simulation  " type="keyboard_vel_ctrl " name="keyboard_vel_ctrl " />

</launch>

gmapping参数设置

<launch>
<include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"   />
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" >
    <param name="maxUrange" value="3.0" />
    <param name="map_update_interval" value="0.5" />
     <param name="linearUpdate" value="0.1" />
</node>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz" />
<node pkg="wpr_simulation  " type="keyboard_vel_ctrl " name="keyboard_vel_ctrl " />
</launch>

保存和加载地图

map_server 是 ROS（Robot Operating System）中的一个重要功能包，主要用于处理地图数据。它提供了两个主要功能：map_server 节点和 map_saver 命令行工具。以下是对其功能的详细解释：

1. map_server 节点
   map_server 节点从磁盘读取地图文件，并通过 ROS 服务提供地图数据。地图数据通常以两种文件格式存储：

• YAML 文件：描述地图的元数据，包括地图的分辨率、原点、阈值等信息。
• 图像文件：通常是 .pgm 或 .png 格式，表示地图的占用情况。图像中的每个像素对应地图中的一个单元格，像素的颜色值表示该单元格的占用状态（自由、占用或未知）。

主要功能：

• 发布地图数据：map_server 节点会发布两个主题：
  • map_metadata：发布地图的元数据（如分辨率、原点等）。
  • map：发布地图的占用网格数据（OccupancyGrid）。
• 提供服务：map_server 还提供一个 static_map 服务，允许其他节点通过服务调用获取地图数据。

使用示例：

rosrun map_server map_server mymap.yaml

这个命令会启动 map_server 节点，并加载 mymap.yaml 文件中指定的地图。

2. map_saver 工具
   map_saver 是一个命令行工具，用于将地图保存到磁盘。它通常用于保存通过 SLAM（同步定位与地图构建）生成的地图。

主要功能：

• 保存地图：map_saver 会从指定的 ROS 主题中读取地图数据，并将其保存为 .pgm 和 .yaml 文件。
  默认主题：/map（消息类型：nav_msgs/OccupancyGrid）
  自定义主题：可以通过 map:=<topic_name> 指定其他主题。
• 自定义阈值：可以通过 --occ 和 --free 参数设置占用和自由区域的阈值。

使用示例：

rosrun map_server map_saver -f mymap

这个命令会将地图保存为 mymap.pgm 和 mymap.yaml 文件。

3. 地图格式
   地图由两个文件组成：

• YAML 文件：包含地图的元数据，如分辨率、原点、阈值等。
• 图像文件：包含地图的占用数据，通常为灰度图像，白色表示自由区域，黑色表示占用区域，灰色表示未知区域。

YAML 文件示例：

image: testmap.png
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

• image：图像文件的路径。

• resolution：地图的分辨率，单位为米/像素。

• origin：地图中左下角像素的二维位姿，格式为 (x, y, yaw)，其中 yaw 表示逆时针旋转（yaw=0 表示无旋转）。目前系统的许多部分都忽略了 yaw。
  

• occupied_thresh 和 free_thresh：分别表示占用和自由区域的阈值。

• negate：是否反转图像的语义（黑白反转）。

4. 值解释
   地图中的像素值可以解释为三种状态：

• Trinary（三元）：像素值被解释为自由（0）、占用（100）或未知（-1）。
• Scale（缩放）：像素值被解释为介于自由和占用之间的渐变值。
• Raw（原始）：直接使用像素的原始值。