ROS实验笔记之——基于cartographer的多机器人SLAM地图融合

Posted 2021-12-10 gwpscut

之前博客《 ROS仿真笔记之——多移动机器人SLAM地图融合 》已经实现了基于gmapping的多机器人地图融合。实验和仿真都验证过了。本博文通过cartographer来实现SLAM，再做map merge

先看视频效果

two

启动的文件

#! /bin/bash
gnome-terminal --tab -e 'bash -c "roscore;exec bash"'

sleep 3s
gnome-terminal --tab -e 'bash -c "expect robot1_run_kobuki.sh;exec bash"'
gnome-terminal --tab -e 'bash -c "expect robot2_run_kobuki.sh;exec bash"'

sleep 3s
gnome-terminal --tab -e 'bash -c "expect robot1_run_rplidar.sh;exec bash"'
gnome-terminal --tab -e 'bash -c "expect robot2_run_rplidar.sh;exec bash"'

sleep 3s
gnome-terminal --tab -e 'bash -c "ROS_NAMESPACE=tb2_0 roslaunch turtlebot_navigation multi_rplidar_cartographer.launch ns:=tb2_0_;exec bash"'
gnome-terminal --tab -e 'bash -c "ROS_NAMESPACE=tb2_1 roslaunch turtlebot_navigation multi_rplidar_cartographer.launch ns:=tb2_1_;exec bash"'

sleep 3s
gnome-terminal --tab -e 'bash -c "roslaunch turtlebot_bringup multi_cartographer_map_merge.launch;exec bash"'


sleep 3s
# gnome-terminal --window -e 'bash -c "ROS_NAMESPACE=tb2_0 roslaunch turtlebot_teleop keyboard_teleop.launch;exec bash"'
# gnome-terminal  --window -e 'bash -c "ROS_NAMESPACE=tb2_1 roslaunch turtlebot_teleop keyboard_teleop.launch;exec bash"'

multi_rplidar_cartographer.launch

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<launch>
  <param name="/use_sim_time" value="false" />
  <!-- 必须改为false（实验时） -->

  <arg name="ns" default=""/>

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename $(arg ns)multi_slam.lua"
      output="screen">
    <remap from="scan" to="scan" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />


 <!-- <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/multi_robot_mapmerge.rviz" /> -->

</launch>

multi_cartographer_map_merge.launch

<launch>
  <arg name="first_tb2"  default="tb2_0"/>
  <arg name="second_tb2" default="tb2_1"/>

  <arg name="first_tb2_x_pos" default="0.0"/>
  <arg name="first_tb2_y_pos" default="0.5"/>
  <arg name="first_tb2_z_pos" default=" 0.0"/>
  <arg name="first_tb2_yaw"   default=" 0.0"/>

  <arg name="second_tb2_x_pos" default="0.0"/>
  <arg name="second_tb2_y_pos" default="-0.5"/>
  <arg name="second_tb2_z_pos" default=" 0.0"/>
  <arg name="second_tb2_yaw"   default=" 0.0"/>


  <group ns="$(arg first_tb2)/map_merge">
    <param name="init_pose_x"   value="$(arg first_tb2_x_pos)"/>
    <param name="init_pose_y"   value="$(arg first_tb2_y_pos)"/>
    <param name="init_pose_z"   value="$(arg first_tb2_z_pos)"/>
    <param name="init_pose_yaw" value="$(arg first_tb2_yaw)"  />
  </group>

  <group ns="$(arg second_tb2)/map_merge">
    <param name="init_pose_x"   value="$(arg second_tb2_x_pos)"/>
    <param name="init_pose_y"   value="$(arg second_tb2_y_pos)"/>
    <param name="init_pose_z"   value="$(arg second_tb2_z_pos)"/>
    <param name="init_pose_yaw" value="$(arg second_tb2_yaw)"  />
</group>


  <node pkg="multirobot_map_merge" type="map_merge" respawn="false" name="map_merge" output="screen">
    <param name="robot_map_topic" value="map"/>
    <param name="robot_namespace" value="tb2"/>
    <param name="merged_map_topic" value="map"/>
    <param name="world_frame" value="map"/>
    <param name="known_init_poses" value="true"/>
    <param name="merging_rate" value="0.5"/>
    <param name="discovery_rate" value="0.05"/>
    <param name="estimation_rate" value="0.1"/>
    <param name="estimation_confidence" value="1.0"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="world_to_$(arg first_tb2)_tf_broadcaster"  args="0 0 0 0 0 0 /map /$(arg first_tb2)/map 100"/>
  <node pkg="tf" type="static_transform_publisher" name="world_to_$(arg second_tb2)_tf_broadcaster" args="0 0 0 0 0 0 /map /$(arg second_tb2)/map 100"/>
  <!-- <node pkg="tf" type="static_transform_publisher" name="world_to_$(arg third_tb2)_tf_broadcaster" args="0 0 0 0 0 0 /map /$(arg second_tb2)/map 100"/> -->


 <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/multi_robot_mapmerge.rviz" />



</launch>


<!-- http://wiki.ros.org/multirobot_map_merge -->

(arg ns)multi_slam.lua

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

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

options = {
  map_builder = MAP_BUILDER,                -- map_builder.lua的配置信息
  trajectory_builder = TRAJECTORY_BUILDER,  -- trajectory_builder.lua的配置信息
  
  map_frame = "tb2_0/map",                        -- 地图坐标系的名字
  tracking_frame = "tb2_0/laser",              -- 将所有传感器数据转换到这个坐标系下
  published_frame = "tb2_0/laser",            -- tf: map -> footprint
  odom_frame = "tb2_0/odom",                      -- 里程计的坐标系名字
  provide_odom_frame = true,               -- 是否提供odom的tf, 如果为true,则tf树为map->odom->footprint
                                            -- 如果为false tf树为map->footprint
  publish_frame_projected_to_2d = false,    -- 是否将坐标系投影到平面上
  use_pose_extrapolator = true,            -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿

  use_odometry = false,                     -- 是否使用里程计,如果使用要求一定要有odom的tf
  use_nav_sat = false,                      -- 是否使用gps
  use_landmarks = false,                    -- 是否使用landmark
  num_laser_scans = 1,                      -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0,           -- 是否使用multi_echo_laser_scans数据
  num_subdivisions_per_laser_scan = 1,      -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 0,                     -- 是否使用点云数据
  
  lookup_transform_timeout_sec = 0.2,       -- 查找tf时的超时时间
  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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
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

-- MAP_BUILDER.use_trajectory_builder_2d = true

-- TRAJECTORY_BUILDER_2D.use_imu_data = true
-- TRAJECTORY_BUILDER_2D.min_range = 0.3
-- TRAJECTORY_BUILDER_2D.max_range = 100.
-- TRAJECTORY_BUILDER_2D.min_z = 0.2
-- --TRAJECTORY_BUILDER_2D.max_z = 1.4
-- --TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02

-- --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
-- --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200.
-- --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.

-- --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9
-- --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
-- --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.

-- TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
-- --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12

-- --TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
-- --TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004
-- --TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1.

-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1

-- POSE_GRAPH.optimize_every_n_nodes = 160.
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- POSE_GRAPH.constraint_builder.min_score = 0.48
-- POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

return options

总结

map merge这个包是基于特征点来做地图融合的。通过视频可以看到，一开始匹配的点可能不是很好，这样导致融合的地图很乱。但是慢慢的，随着机器人走动的路径越来越多（而cartographer的回环也让地图越来越好）最终融合的效果还是不错的。

初始的位置影响其实不是很大～～～