亚博microros小车-原生ubuntu支持系列：17 gmapping

前置依赖

先看下亚博官网的介绍

Gmapping简介

gmapping只适用于单帧二维激光点数小于1440的点，如果单帧激光点数大于1440，那么就会出【[mapping-4] process has died】这样的问题。

Gmapping是基于滤波SLAM框架的常用开源SLAM算法。

Gmapping基于RBpf粒子滤波算法，即时定位和建图过程分离，先进行定位再进行建图。

Gmapping在RBpf算法上做了两个主要的改进：改进提议分布和选择性重采样。

优点：Gmapping可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。

缺点：随着场景增大所需的粒子增加，因为每个粒子都携带一幅地图，因此在构建大地图时所需内存和计算量都会增加。因此不适合构建大场景地图。并且没有回环检测，因此在回环闭合时可能会造成地图错位，虽然增加粒子数目可以使地图闭合但是以增加计算量和内存为代价。

底层转换

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py      #底层数据程序

参见：https://blog.csdn.net/bohu83/article/details/145394204

ros2 launch yahboomcar_nav map_gmapping_launch.py    #建图节点

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    slam_gmapping_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('slam_gmapping'), 'launch'),
         '/slam_gmapping.launch.py'])
    )

    base_link_to_laser_tf_node = Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_laser',
     arguments=['-0.0046412', '0' , '0.094079','0','0','0','base_link','laser_frame']
    )
    
    return LaunchDescription([slam_gmapping_launch,base_link_to_laser_tf_node])

这里启动了一个launch文件-slam_gmapping_launch和一个发布静态变换的节点-base_link_to_laser_tf_node。

从代码上看，依赖了slam_gmapping.

slam_gammping

可以从网上找个资料大概了解下，也可以看官网文档：gmapping - ROS Wiki

从文档大概看就是订阅话题：/scan 激光雷达数据、tf 坐标变换；发布了话题:map_metadata、map\~entropy 服务：dynamic_map 获取地图数据

launch还是要引用代码，所以apt-install 办法不适合,需要找源码编译安装。

https://github.com/ros-perception/slam_gmapping

找了这个地址，试了下编译安装。

CMake Error at CMakeLists.txt:5 (find_package):
  By not providing "Findcatkin.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "catkin", but
  CMake did not find one.

  Could not find a package configuration file provided by "catkin" with any
  of the following names:

    catkinConfig.cmake
    catkin-config.cmake

  Add the installation prefix of "catkin" to CMAKE_PREFIX_PATH or set
  "catkin_DIR" to a directory containing one of the above files.  If "catkin"
  provides a separate development package or SDK, be sure it has been
  installed.

我的测试环境是ubuntu 22+ ros2 humble.系统缺少 catkin 这个包.

因为在ros2 中catkin 已经被ament取代，如何用ros2 中colcon build 编译 ros中 catkin的功能包？

需要修改源码和cmakelists把catkin相关的更换成ament，我看了下比较麻烦，直接使用了亚博官方的代码。工程结构如下所示，就是指定了启动脚本:src/slam_gmapping/launch/slam_gmapping.launch.py跟slam_gmapping.yaml

slam_gmapping.launch.py，这里启动了slam_gmapping的节点，加载了slam_gmapping.yaml参数文件。

slam_gmapping.yaml

/slam_gmapping:
  ros__parameters:
    angularUpdate: 0.5
    astep: 0.05
    base_frame: base_footprint
    map_frame: map
    odom_frame: odom
    delta: 0.05
    iterations: 5
    kernelSize: 1
    lasamplerange: 0.005
    lasamplestep: 0.005
    linearUpdate: 1.0
    llsamplerange: 0.01
    llsamplestep: 0.01
    lsigma: 0.075
    lskip: 0
    lstep: 0.05
    map_update_interval: 5.0
    maxRange: 6.0
    maxUrange: 4.0
    minimum_score: 0.0
    occ_thresh: 0.25
    ogain: 3.0
    particles: 30
    qos_overrides:
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_all
          reliability: reliable
      /tf:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    resampleThreshold: 0.5
    sigma: 0.05
    srr: 0.1
    srt: 0.2
    str: 0.1
    stt: 0.2
    temporalUpdate: 1.0
    transform_publish_period: 0.05
    use_sim_time: false
    xmax: 10.0
    xmin: -10.0
    ymax: 10.0
    ymin: -10.0

运行

启动小车代理：

首先启动小车处理底层数据程序，

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():
    package_path = get_package_share_path('yahboomcar_nav')
    default_rviz_config_path = package_path / 'rviz/view.rviz'
    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
                                     description='Absolute path to rviz config file')

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return LaunchDescription([
        rviz_arg,
        rviz_node
    ])

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-30-14-52-59-130561-bohu-TM1701-316456
[INFO] [launch]: Default logging verbosity is set to INFO
---------------------robot_type = x3---------------------
[INFO] [complementary_filter_node-1]: process started with pid [316458]
[INFO] [ekf_node-2]: process started with pid [316460]
[INFO] [static_transform_publisher-3]: process started with pid [316462]
[INFO] [joint_state_publisher-4]: process started with pid [316464]
[INFO] [robot_state_publisher-5]: process started with pid [316466]
[INFO] [static_transform_publisher-6]: process started with pid [316468]
[static_transform_publisher-3] [WARN] [1738219979.422738190] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-6] [WARN] [1738219979.430808316] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-3] [INFO] [1738219979.474861598] [base_link_to_base_imu]: Spinning until stopped - publishing transform
[static_transform_publisher-3] translation: ('-0.002999', '-0.003000', '0.031701')
[static_transform_publisher-3] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-3] from 'base_link' to 'imu_frame'
[static_transform_publisher-6] [INFO] [1738219979.486498703] [static_transform_publisher_dyynkHgPuvMB4QSZ]: Spinning until stopped - publishing transform
[static_transform_publisher-6] translation: ('0.000000', '0.000000', '0.050000')
[static_transform_publisher-6] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-6] from 'base_footprint' to 'base_link'
[complementary_filter_node-1] [INFO] [1738219979.494221633] [complementary_filter_gain_node]: Starting ComplementaryFilterROS
[robot_state_publisher-5] [WARN] [1738219979.497129656] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-5] [INFO] [1738219979.497311822] [robot_state_publisher]: got segment base_link
[robot_state_publisher-5] [INFO] [1738219979.497405894] [robot_state_publisher]: got segment imu_Link
[robot_state_publisher-5] [INFO] [1738219979.497427256] [robot_state_publisher]: got segment jq1_Link
[robot_state_publisher-5] [INFO] [1738219979.497440404] [robot_state_publisher]: got segment jq2_Link
[robot_state_publisher-5] [INFO] [1738219979.497453384] [robot_state_publisher]: got segment radar_Link
[robot_state_publisher-5] [INFO] [1738219979.497465220] [robot_state_publisher]: got segment yh_Link
[robot_state_publisher-5] [INFO] [1738219979.497477294] [robot_state_publisher]: got segment yq_Link
[robot_state_publisher-5] [INFO] [1738219979.497489516] [robot_state_publisher]: got segment zh_Link
[robot_state_publisher-5] [INFO] [1738219979.497501480] [robot_state_publisher]: got segment zq_Link
[joint_state_publisher-4] [INFO] [1738219980.049008205] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...

然后，启动rviz，可视化建图，终端输入，

ros2 launch yahboomcar_nav display_launch.py

此时还没运行建图节点，所以没有数据。接下来运行建图节点，终端输入，

ros2 launch yahboomcar_nav map_gmapping_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    slam_gmapping_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
        get_package_share_directory('slam_gmapping'), 'launch'),
         '/slam_gmapping.launch.py'])
    )

    base_link_to_laser_tf_node = Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_laser',
     arguments=['-0.0046412', '0' , '0.094079','0','0','0','base_link','laser_frame']
    )
    
    return LaunchDescription([slam_gmapping_launch,base_link_to_laser_tf_node])

#启动键盘控制
ros2 run yahboomcar_ctrl yahboom_keyboard

控制小车慢慢转一圈。最终效果如下

生成的map

建图完毕后，输入以下指令保存地图，终端输入，

ros2 launch yahboomcar_nav save_map_launch.py

from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node
import os


def generate_launch_description():
    package_share_path = str(get_package_share_path('yahboomcar_nav'))
    package_path = os.path.abspath(os.path.join(
        package_share_path, "../../../../src/yahboomcar_nav"))
    map_name = "yahboom_map"
    default_map_path = os.path.join(package_path, 'maps', map_name)

    map_arg = DeclareLaunchArgument(name='map_path', default_value=str(default_map_path),
                                    description='The path of the map')

    map_saver_node = Node(
        package='nav2_map_server',
        executable='map_saver_cli',
        arguments=[
            '-f', LaunchConfiguration('map_path'), '--ros-args', '-p', 'save_map_timeout:=60000.00'],
    )

    return LaunchDescription([
        map_arg,
        map_saver_node
    ])

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_nav save_map_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-30-15-33-14-161314-bohu-TM1701-319017
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_saver_cli-1]: process started with pid [319018]
[map_saver_cli-1] [INFO] [1738222394.419546962] [map_saver]: 
[map_saver_cli-1] 	map_saver lifecycle node launched. 
[map_saver_cli-1] 	Waiting on external lifecycle transitions to activate
[map_saver_cli-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_saver_cli-1] [INFO] [1738222394.419654445] [map_saver]: Creating
[map_saver_cli-1] [INFO] [1738222394.419713380] [map_saver]: Configuring
[map_saver_cli-1] [INFO] [1738222394.420777030] [map_saver]: Saving map from 'map' topic to '/home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map' file
[map_saver_cli-1] [WARN] [1738222394.420801411] [map_saver]: Free threshold unspecified. Setting it to default value: 0.250000
[map_saver_cli-1] [WARN] [1738222394.420813330] [map_saver]: Occupied threshold unspecified. Setting it to default value: 0.650000
[map_saver_cli-1] [WARN] [1738222395.293844583] [map_io]: Image format unspecified. Setting it to: pgm
[map_saver_cli-1] [INFO] [1738222395.294086677] [map_io]: Received a 384 X 608 map @ 0.05 m/pix
[map_saver_cli-1] [INFO] [1738222395.473549155] [map_io]: Writing map occupancy data to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.pgm
[map_saver_cli-1] [INFO] [1738222395.474901659] [map_io]: Writing map metadata to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.yaml
[map_saver_cli-1] [INFO] [1738222395.475071124] [map_io]: Map saved
[map_saver_cli-1] [INFO] [1738222395.475082707] [map_saver]: Map saved successfully
[map_saver_cli-1] [INFO] [1738222395.737027994] [map_saver]: Destroying
[INFO] [map_saver_cli-1]: process has finished cleanly [pid 319018]

会有两个文件生成，一个是yahboom_map.pgm，一个是yahboom_map.yaml，看下yaml的内容，

image: yahboom_map.pgm

mode: trinary

resolution: 0.05

origin: [-10, -10, 0]

negate: 0

occupied_thresh: 0.65

free_thresh: 0.25