ROS2智能机器人开发实践-阅读分享8-cartograph建图

zyzyzyzyZYZyyy

ROS2智能机器人开发实践-阅读分享8-cartograph建图 [复制链接]

 

ROS2智能机器人开发实践-阅读分享8-cartograph建图

在讲这个之前我们需要了解slam的主要建图方向，滤波和图优化。

SLAM问题主要分为滤波和图优化两大类。以扩展卡尔曼滤波为例，滤波就是通过当前时刻状态+输入+运动模型来估计下一时刻的状态，然后通过观测模型来纠正，因为其计算量小，只考虑相邻帧，因此被广泛应用到嵌入式中。在图优化（graph-based slam）的方法中，处理数据的方式就和滤波不同了，它不是在线的纠正位姿，而是把所有的数据记录下来，最后一次算总账。

可以参考文章如下：【转】graph slam tutorial：1 什么是图优化 - 知乎

这些细节当然很复杂，咱们就直接使用代码来建图吧。由于使用slam-toolbox有点问题我们直接使用cartographer来进行建图仿真。

本文前置环境介绍，up主手上的开发板是rdkx3,刷了官方的20.04+foxy版本（其实也不是官方版本，而是地平线智能车竞赛的版本，里面有内置的初始化ros2代码。ros2 launch origincar_base origincar_bringup.launch.py 主要是启动和stm32板子的联系并且发布/odom等话题，毕竟建图有一些依赖，毕竟我这不是仿真，大家可以看一看手头上有没有啥小车）

我的激光雷达使用的是乐动的stl06nbj，这个型号应该是stl06p同一个版本，我使用的是相同的驱动代码没啥问题。AlgoPathfinder/sdk_ldrobotsensorteam_stl: sdk for ldrobot lidar这是github历程（主要是这个激光雷达二手99也挺便宜的，使用一个ch340的串口就可以读取了）

然后我的的嵌入式板子使用的是轮趣科技的stm32的ros开发板（四驱的版本）

自己搞了一个四轮的麦克纳木轮小车

大家有任何疑问可以询问我，分享可能不够仔细。

重要参考文章：10.5配置Fishbot进行建图

注意注意这是foxy版本，大家的jazzy版本大致相同但是总有一些可能不同，遇到问题可以自己搜索。

其次就是本书内容（书本上的注释更多但是步骤有点不详细）

ros2 pkg create mycar_cartographer
cd mycar_cartographer
mkdir config
mkdir launch
mkdir rviz

在config目录下创建mycar_2d.lua文件,这是一个建图的配置文件，我们来一一解释。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
--下面两个不过多解释了
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
--下面5个仔细解释，看下文
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom_combined",
odom_frame = "odom_combined",
provide_odom_frame = false,


-- false改为true，仅发布2D位资
publish_frame_projected_to_2d = true,
-- false改为true，使用里程计数据，这里需要使用里程计话题
use_odometry = true,
--是否使用gps
use_nav_sat = false,
--是否使用路标
use_landmarks = false,
-- 0改为1,使用一个雷达
num_laser_scans = 1,
-- 1改为0，不使用多波雷达
num_multi_echo_laser_scans = 0,
-- 10改为1，1/1=1等于不分割，将一次/scan分成一次一次来处理
num_subdivisions_per_laser_scan = 1,
--是否使用点云数据（这个应该是深度相机）
num_point_clouds = 0,
--查找tf变换坐标的超时时间
lookup_transform_timeout_sec = 0.2,
--发布submap子图的周期，单位为s
submap_publish_period_sec = 0.3,
--发布姿态的周期
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}


-- false改为true，启动2D SLAM
MAP_BUILDER.use_trajectory_builder_2d = true

-- 0改成0.10,比机器人半径小的都忽略
TRAJECTORY_BUILDER_2D.min_range = 0.15
-- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些
TRAJECTORY_BUILDER_2D.max_range = 3.5
-- 5改成3,传感器数据超出有效范围最大值
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
-- 不使用IMU数据，大家可以开启，然后对比下效果(这里如果开启会有一些问题)
-- 我的tf节点里面没有imu_link(他的名字是groy_link貌似是)，这里必须要imu_link
-- 而且建议imu_link和base_link之间转换为零平移（重合）
TRAJECTORY_BUILDER_2D.use_imu_data = false
-- false改成true,使用实时回环检测来进行前端的扫描匹配
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
-- 1.0改成0.1,提高对运动的敏感度
-- 仅当角度变化超过0.1弧度（~5.7°）时处理新扫描，减少冗余计算
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

-- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。
POSE_GRAPH.constraint_builder.min_score = 0.65
--0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

-- 设置0可关闭全局SLAM
-- POSE_GRAPH.optimize_every_n_nodes = 0

return options

map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,

以下的解释由deepseek解释：

这几个参数涉及到ROS中的坐标系（TF）设置，理解它们的含义对于正确配置SLAM系统非常重要。首先我们先看一下我们的tf图和节点图再来讨论。

1. map_frame

• 含义：map_frame 是地图的坐标系名称，通常为 "map"。
• 作用：SLAM系统会构建一个全局地图，并将机器人定位到这个地图中。map_frame 就是这个全局地图的坐标系。
• 实际情况：如果你的系统中已经有 map 坐标系（例如通过其他SLAM系统或手动发布），确保这里的 map_frame 名称与现有系统一致即可。
  2. tracking_frame
• 含义：tracking_frame 是机器人本体的坐标系名称，通常为 "base_link"。
• 作用：SLAM系统需要知道机器人本体的坐标系，以便将传感器数据（如激光雷达、IMU等）与机器人本体对齐。
• 实际情况：我的机器人本体坐标系是 "base_link"，这里不需要修改。
  3. published_frame
• 含义：published_frame 是SLAM系统发布的坐标系名称，这里设置为 "odom_combined"。
• 作用：SLAM系统会发布从 map_frame 到 published_frame 的位姿变换（即 map 到 odom_combined 的变换）。这个变换表示机器人在地图中的位置。
• 实际情况：如果你已经有 odom_combined（通过EKF融合得到的里程计），可以将 published_frame 设置为 "odom_combined"，这样SLAM系统会发布 map 到 odom_combined 的变换。
  4. odom_frame
• 含义：odom_frame 是里程计的坐标系名称，通常为 "odom"。
• 作用：SLAM系统需要知道里程计的坐标系，以便将里程计数据与地图对齐。
• 实际情况：如果你已经有 odom_combined，可以将 odom_frame 设置为 "odom_combined"，这样SLAM系统会使用 odom_combined 作为里程计的坐标系。
  5. provide_odom_frame
• 含义：provide_odom_frame 表示是否由SLAM系统提供 odom_frame。
• 作用：
  • 如果设置为 true，SLAM系统会发布 odom_frame 到 base_link 的变换。
  • 如果设置为 false，SLAM系统不会发布 odom_frame 到 base_link 的变换，而是依赖外部系统（如EKF）提供这个变换。
• 实际情况：因为你已经有 odom_combined（通过EKF融合得到的里程计），所以这里应该设置为 false，表示SLAM系统不提供 odom_frame 到 base_link 的变换，而是使用 odom_combined。
  我们来看一下实际的建图过程
  [传感器数据源]
  │
  ├─── /laserscan（激光雷达数据）
  │
  ├─── /odom（里程计数据）
  │
  └─── /imu（IMU惯性测量单元数据）
  │
  │
  ▼
  /cartographer_node（Cartographer核心处理节点）
  │
  ├──── /submap_list（实时子地图列表）
  │
  └──── /occupancy_grid_node（最终栅格地图）
  │
  ▼
  /map（可视化地图）
  
  [TF 坐标系流]
  map（全局地图坐标系） → odom（里程计坐标系） → base_link（机器人本体坐标系）
  ▲ ▲
  └───── 由 Cartographer 发布 ────────────┘
  在路径src/mycar_cartographer/launch/下新建cartographer.launch.py文件，接着我们将cartographer两个节点加入到这个launch文件中。
  import os
  from launch import LaunchDescription
  from launch.substitutions import LaunchConfiguration
  from launch_ros.actions import Node
  from launch_ros.substitutions import FindPackageShare
  
  
  def generate_launch_description():
  # 定位到功能包的地址
  pkg_share = FindPackageShare(package='mycar_cartographer').find('mycar_cartographer')
  
  #=====================运行节点需要的配置=======================================================================
  # 是否使用仿真时间，我们用不使用gazeboo这里填写false
  use_sim_time = LaunchConfiguration('use_sim_time', default='false')
  # 地图的分辨率
  resolution = LaunchConfiguration('resolution', default='0.05')
  # 地图的发布周期
  publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
  # 配置文件夹路径
  configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
  # 配置文件
  configuration_basename = LaunchConfiguration('configuration_basename', default='mycar_2d.lua')
  
  
  #=====================声明两个个节点，cartographer/occupancy_grid_node=================================
  cartographer_node = Node(
  package='cartographer_ros',
  executable='cartographer_node',
  name='cartographer_node',
  output='screen',
  parameters=[{'use_sim_time': use_sim_time}],
  arguments=[
  '-configuration_directory', configuration_directory,
  '-configuration_basename', configuration_basename
  ],
  # 添加重映射规则（格式：原始话题 → 新话题）
  remappings=[
  ('/scan', '/scan'), # 激光雷达数据
  ('/odom', '/odom_combined'), # 里程计数据
  # ('/imu', '/imu/data') # IMU 数据（按需添加）
  ]
  )
  
  occupancy_grid_node = Node(
  package='cartographer_ros',
  executable='occupancy_grid_node',
  name='occupancy_grid_node',
  output='screen',
  parameters=[{'use_sim_time': use_sim_time}],
  arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
  
  
  #定义启动文件
  ld = LaunchDescription()
  ld.add_action(cartographer_node)
  ld.add_action(occupancy_grid_node)
  
  
  return ld
  这里没有rviz2,大家可以自己在虚拟机执行。
  打开CmakeLists.txt，添加下面一条指令，将三个目录安装到install目录。
  install(
  DIRECTORY config launch rviz
  DESTINATION share/${PROJECT_NAME}
  )
  #编译执行
  colcon build --packages-select mycar_cartographer
  source install/setup.bash
  ros2 launch mycar_cartographer cartographer.launch.py
  我们打开rviz2来看一看咯
  
  看见map显示没什么问题，我现在放在地上来跑一下。（在宿舍没啥位置后面去社团再试一试）
  然后我们来保存一下地图
  sudo apt install ros-foxy-nav2-map-server
  先将地图保存到src/mycar_cartographer/map目录下
  ros2 run nav2_map_server map_saver_cli -t map -f mycar_map
  10.6导航地图概述
  地图文件保存后我们可以直接加载使用了
  本文tip（我的开发板上面有有好几个usb设备）
  查看串口
• ls命令：
  ls -l /dev/tty*
• 查看有哪些设备连接在你的电脑上:
  lsusb
• 产看串口个数以及对应的tty:
  dmesg | grep ttyS*

查看本帖全部内容，请登录或者注册

Jacktang

用代码来建图效果还是很不错的