Cartographer使用实录

Cartographer使用实录

本文使用velodyne 32线激光雷达加IMU实现无里程计3维SLAM建图

cartographer安装

参照https://google-cartographer.readthedocs.io/en/latest/进行

cartographer修改参数

1.创建urdf文件

<!--
  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.
-->

<robot name="cartographer_backpack_3d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

 <link name="imu">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0.01 0. 0.19" rpy="0 0 0" />
  </joint>

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

</robot>

以base_link为底盘坐标系，通过测试，确定IMU与laser的坐标位置关系，默认IMU与底盘位置一直，修改完成，命名为my_robot.urdf
2.创建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,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,--不使用里程计
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,--3维激光雷达数量为一
  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 = 1,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,--imu采取100%
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 10--要累积到一个未翘曲的组合范围数据中的范围数据的数量 用于扫描匹配，不可过大，过大会导致定位数据更新过慢，导致定位丢失。

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

3.创建launch文件

<launch>
  <!-- 要跑自己的实际数据，这里务必设置为false -->
  <!-- <param name="/use_sim_time" value="false" /> -->
  <!-- 需要包含自己设置的URDF描述文件 -->
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_robot.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
  <!-- <node name="joint_state_publisher" pkg="joint_state_publisher" 
    type="joint_state_publisher" /> -->

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_robot.lua"
      output="screen">
      <!-- 上面的 configuration_basename 需要修改为自己对应的lua文件名-->
    <!-- 下面需要修改为雷达数据的话题 -->
    <remap from="points2" to="/horizontal_laser_3d" />
    <!-- 发布的imu话题 /mavros/imu/data -->
    <remap from="imu" to="/imu/data" />
  </node>

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

  <!-- lx_3d.rviz 中添加了3d点云的显示  (他编写了对应的代码实现的) -->
  <!-- 如果下载的是官方的cartographer代码，需要修改为 demo_3d.rviz -->
  <!-- <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/lx_3d.rviz" /> -->

</launch>

修改points2的话题名和imu话题名与lua文件与urdf文件位置

随后运行即可实现地图扫描

地图保存

1.停止建图

当建图完成后，输入以下命令停止地图构建。

rosservice call /finish_trajectory 0

2.保存地图

输入以下命令保存地图。

rosservice call /write_state "{filename: '/home/me/map/test.pbstream', include_unfinished_submaps: "false"}"

3.将cartographer保存的地图转换

转换为为ROS格式下的栅格地图

rosrun cartographer_ros cartographer_pbstream_to_ros_map -pbstream_filename=<绝对路径>/<地图名字>.pbstream -map_filestem=<绝对路径>/<地图名字>

导航与定位