使用激光雷达和rf2o算法在cartographer建图出现漂移重叠

1039291016

背景：

最近在搞激光雷达建图，使用的是思岚a2m12激光雷达，然后尝试直接使用单激光雷达建图，发现地图漂移重叠比较严重，查询后得知可能需要里程计来提高精度，但由于我们的小车结构还在改，没确定最后方案，因此使用rf2o算法来获取里程计数据，结果使用过程就出现了两个问题
首先先将相关代码文件放上来吧

雷达的launch文件
def generate_launch_description():

    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    urdf_dir = os.path.join(pkg_share, 'urdf')
    urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf')
    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()

    ## ***** Nodes *****
    robot_state_publisher_node = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        parameters=[
            {'robot_description': robot_desc},
            {'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output = 'screen'
        )

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'backpack_2d.lua'],
        remappings = [('scan','scan'),
            ('odom','odom_rf2o'),
            ('odom','odom_rf2o')],#('scan','scan')('odom','/odom_rf2o')('imu','/camera/camera/imu')echoes,horizontal_laser_2d
        output = 'screen'
        )
        
    # 可视化节点
    rviz_node = Node(
        package='rviz2',
        namespace='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen')

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': False},
            {'resolution': 0.05}],
        )

    return LaunchDescription([
        use_sim_time_arg,
        # Nodes
        robot_state_publisher_node,
        rviz_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

rf2o的launch文件
def generate_launch_description():

    return LaunchDescription([

            Node(
                package='rf2o_laser_odometry',
                executable='rf2o_laser_odometry_node',
                name='rf2o_laser_odometry',
                output='screen',
                parameters=[{
                    'laser_scan_topic' : '/scan',
                    'odom_topic' : '/odom_rf2o',#/odom_rf2o
                    'publish_tf' : True,
                    'base_frame_id' : 'base_link',
                    'odom_frame_id' : 'odom',
                    'init_pose_from_topic' : '',
                    'freq' : 10.0}],
            ),
    ])

cartograoher的launch文件
def generate_launch_description():

    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    urdf_dir = os.path.join(pkg_share, 'urdf')
    urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf')
    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()

    ## ***** Nodes *****
    robot_state_publisher_node = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        parameters=[
            {'robot_description': robot_desc},
            {'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output = 'screen'
        )

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'backpack_2d.lua'],
        remappings = [('scan','scan'),
            ('odom','odom_rf2o'),
            ('odom','odom_rf2o')],#('scan','scan')('odom','/odom_rf2o')('imu','/camera/camera/imu')echoes,horizontal_laser_2d
        output = 'screen'
        )
        
    # 可视化节点
    rviz_node = Node(
        package='rviz2',
        namespace='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen')

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': False},
            {'resolution': 0.05}],
        )

    return LaunchDescription([
        use_sim_time_arg,
        # Nodes
        robot_state_publisher_node,
        rviz_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

cartographer的lua文件
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",--camera_imu_optical_frame base_link
  published_frame = "odom",-- laser_frame base_link
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,-- 10
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  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 = 0.5,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 35 --10
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

问题一：
当我配置好文件然后建图时，cartographer弹出了这样的报错：

[cartographer_node-3] F0716 14:47:49.030234 58418 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (638567092688185279 vs. 638567092688185279) 
[cartographer_node-3] [FATAL] [1721112469.031000008] [cartographer logger]: F0716 14:47:49.000000 58418 map_by_time.h:43] Check failed: data.time > std::prev(trajectory.end())->first (638567092688185279 vs. 638567092688185279) 
[cartographer_node-3] F0716 14:47:49.132308 58386 imu_tracker.cc:67] Check failed: (orientation_ * gravity_vector_).z() > 0. (-nan vs. 0) 
[cartographer_node-3] [FATAL] [1721112469.132508442] [cartographer logger]: F0716 14:47:49.000000 58386 imu_tracker.cc:67] Check failed: (orientation_ * gravity_vector_).z() > 0. (-nan vs. 0) 
[cartographer_node-3] *** Check failure stack trace: ***
[cartographer_node-3]     @     0x757905eb2b03  google::LogMessage::Fail()
[cartographer_node-3]     @     0x757905eba9d1  google::LogMessage::SendToLog()
[cartographer_node-3]     @     0x757905eb27c2  google::LogMessage::Flush()
[cartographer_node-3]     @     0x757905eb478f  google::LogMessageFatal::~LogMessageFatal()
[cartographer_node-3]     @     0x62b1b0616669  cartographer::mapping::optimization::OptimizationProblem2D::AddOdometryData()
[cartographer_node-3]     @     0x62b1b05a4143  _ZNSt17_Function_handlerIFN12cartographer7mapping8WorkItem6ResultEvEZNS1_11PoseGraph2D15AddOdometryDataEiRKNS0_6sensor12OdometryDataEEUlvE_E9_M_invokeERKSt9_Any_data
[cartographer_node-3]     @     0x62b1b05a3af9  cartographer::mapping::PoseGraph2D::DrainWorkQueue()
[cartographer_node-3]     @     0x62b1b066fac5  cartographer::common::Task::Execute()
[cartographer_node-3]     @     0x62b1b0590381  cartographer::common::ThreadPool::DoWork()
[cartographer_node-3]     @     0x7579052dc253  (unknown)
[cartographer_node-3]     @     0x757904e94ac3  (unknown)
[cartographer_node-3]     @     0x757904f26850  (unknown)
[ERROR] [cartographer_node-3]: process has died [pid 58386, exit code -6, cmd '/home/murphy/fishbot_ws/install/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/murphy/fishbot_ws/install/cartographer_ros/share/cartographer_ros/configuration_files -configuration_basename backpack_2d.lua --ros-args --params-file /tmp/launch_params_fm5bsshm -r scan:=scan -r odom:=odom_rf2o -r odom:=odom_rf2o'].

这似乎是雷达的scan和rf2o的odom信息转换间的时间戳出现了问题，但网上查询后有一个解决方法，就是将cartographer的luan文件中的参数odometry_sampling_ratio = 1.,，这个odom采样率从1.0降低，降低到0.51左右就不会报错了，但我认为这种方法实际上是丢失了大量的odom信息，值时将原有问题掩盖而已，所以想问问各位是否遇见过这个问题，有没有什么办法解决？

问题二：
第二个问题是我在将问题一暂且搁置，采用降低采样率进行建图后出现的问题，就是发现即使加上了里程计，其地图依旧出现漂移和重叠，感觉和没加上odom没有区别，这又是怎么一回事呢？望各位大神解答
![地图漂移重叠]( 图片地址)
![tf树]( 图片地址)
![话题节点订阅关系]( 图片地址)

小鱼

@1039291016 我之前测试的效果都比较不错,建图的时候旋转要慢一些,也可以试试 slam_toobox 方案

机器人小伊

针对您遇到的问题，我将分别给出可能的解决方案和建议。

问题一：时间戳问题

报错信息表明 Cartographer 在处理来自激光雷达和 RF2O 提供的里程计数据时，时间戳发生了冲突或不一致。map_by_time.h 和 imu_tracker.cc 中的检查失败通常与数据的时间戳处理有关。

确保时间同步：首先，确保激光雷达和 RF2O 节点都使用相同的时间源（如果可能，使用硬件时间戳）。如果激光雷达和 RF2O 节点运行在仿真中，请确保仿真时间是同步的，并检查 use_sim_time 参数是否正确设置。
检查时间戳的生成：确保 RF2O 生成的里程计消息的时间戳是递增的，并且与激光雷达扫描的时间戳保持合理的一致性。如果 RF2O 的输出频率远低于激光雷达的扫描频率，可能需要插值或调整时间戳以匹配。
调整采样率：虽然降低 odometry_sampling_ratio 可以暂时避免错误，但这并不是长期解决方案。考虑在保持精度的同时增加 RF2O 的输出频率，或者对激光雷达数据进行下采样以匹配 RF2O 的频率。
检查配置参数：确保 Cartographer 的配置文件中其他与时间相关的参数（如 lookup_transform_timeout_sec）设置得当。

问题二：地图漂移和重叠

如果在使用里程计数据后仍然出现地图漂移和重叠，这通常是由于以下几个原因：

里程计精度：RF2O 提供的里程计数据可能不够精确，导致 Cartographer 无法准确估计机器人的位置。考虑使用其他里程计算法或传感器（如 IMU）来提高精度。
配置问题：检查 Cartographer 的配置文件，特别是与扫描匹配、位姿图优化等相关的参数。可能需要调整这些参数以适应您的具体硬件和环境。
初始化问题：确保机器人在建图开始前有一个合理的初始位置和朝向。错误的初始位置可能导致整个地图的漂移。
环境问题：某些环境特征（如重复的结构、缺乏纹理的区域）可能导致扫描匹配失败，从而导致地图漂移。尝试在不同的环境中测试您的系统，并观察是否仍然出现漂移。
TF树和话题关系：确保 TF 树正确设置，并且 Cartographer 节点订阅了正确的里程计和激光雷达话题。您可以使用 rosrun tf view_frames 命令来可视化 TF 树，并使用 rostopic echo 命令来检查话题数据。
硬件问题：最后，考虑检查激光雷达和里程计传感器的硬件连接和校准。硬件问题（如松动的连接线、传感器故障等）也可能导致地图漂移。

以上为生成回答，仅供参考～

小鱼

@1039291016 在使用激光雷达和rf2o算法在cartographer建图出现漂移重叠中说：

rf2o

使用 rf2o 不如直接用carto纯激光建图

1039291016

@小鱼一开始就是使用carto纯激光建图，但地图也会漂移重叠

小鱼

@1039291016 我之前测试的效果都比较不错,建图的时候旋转要慢一些,也可以试试 slam_toobox 方案

1039291016

@小鱼 ok，我再试试纯激光建图的方案，还有slam_toolbox的方案有相关教程或者例子吗？想学习一下，感谢小鱼

1039291016

@小鱼在使用激光雷达和rf2o算法在cartographer建图出现漂移重叠中说：

@1039291016 在使用激光雷达和rf2o算法在cartographer建图出现漂移重叠中说：

rf2o

使用 rf2o 不如直接用carto纯激光建图

还有想问问为啥不推荐使用rf2o，是因为精度很差吗？

小鱼

@1039291016 从本质上来讲,并没有增加传感器

小鱼

@1039291016 直接搜,有很多,社区的fishbot资料中也有

1039291016

@小鱼 ok，感谢小鱼