使用cartographer扩展地图

说明

主要是使用cartographer在已有地图的基础上扩展未知区域地图，避免对已有地图重复建图

1. 加载已有地图

首先要在定位模式下加载已有地图

启动launch文件

bash 复制代码

<launch>
    <arg name = "configuration_directory" default = "/work/slam/script"/>
  <arg name = "configuration_basename" default = "expand_map.lua"/>
  <arg name = "load_state_filename" default = "/work/slam/maps/map.pbstream"/>
  <arg name="load_frozen_state" default="true"/>
 
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(arg configuration_directory)
          -configuration_basename $(arg configuration_basename)
          -load_state_filename $(arg load_state_filename)
          -load_frozen_state $(arg load_frozen_state)"
      output="screen">
  </node>
  <node name="rviz" pkg="rviz" type="rviz" required="true"  
        args="-d /work/slam/script/demo_2d.rviz" />
  </launch>

2. 结束当前轨迹

需要结束定位产生的轨迹

bash 复制代码

source install_isolated/setup.bash
rosservice call /finish_trajectory 1

3. 开启新的轨迹

通过调用cartographer服务开启新的轨迹

bash 复制代码

rosservice call /start_trajectory "configuration_directory: '/work/slam/script'
configuration_basename: 'expand_map.lua'
use_initial_pose: true     
initial_pose:
  position: {x: -0.0551091, y: -0.939531, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: -0.00436956, w: 0.999996}
relative_to_trajectory_id: 0"

4. 保存地图

当对未知地图完成扫描后保存地图

bash 复制代码

#!/bin/bash
source install_isolated/setup.bash

# Call the ROS service and save the output to a variable
response=$(rosservice call /get_trajectory_states "{}")

# Extract the trajectory_id array from the response
trajectory_ids=$(echo "$response" | awk -F'trajectory_id: ' '{print $2}' | awk -F']' '{print $1}')

# Extract the last element from the trajectory_id array
last_element=$(echo "$trajectory_ids" | awk -F', ' '{print $NF}')

# Print the last element
echo "Last element of trajectory_id: $last_element"

rosservice call /finish_trajectory $last_element

rosservice call /write_state "{filename: '/work/slam/map/map.pbstream'}"

注意

1, 由于在定位阶段加载了lua参数，新建轨迹时如果参数不同，不会生效；

2, 在定位模式下，结束当前轨迹时，会删除当前轨迹，导致在转换地图时出现

bash 复制代码

F0317 08:40:01.665091 111541 proto_stream_deserializer.cc:70] Check failed: pose_graph_.pose_graph().trajectory_size() == all_trajectory_builder_options_.all_trajectory_builder_options() .options_with_sensor_ids_size() (2 vs. 1) 
*** Check failure stack trace: ***
    @     0x7a369d2e90cd  google::LogMessage::Fail()
    @     0x7a369d2eaf33  google::LogMessage::SendToLog()
    @     0x7a369d2e8c28  google::LogMessage::Flush()
    @     0x7a369d2eb999  google::LogMessageFatal::~LogMessageFatal()
    @     0x6503b3f07528  (unknown)
    @     0x6503b3ef915a  (unknown)
    @     0x6503b3ef7968  (unknown)
    @     0x7a369ba21c87  __libc_start_main
    @     0x6503b3ef901a  (unknown)

因此需要在代码中对删除的操作进行特别处理