Ubuntu20.04使用速腾16线激光雷达与IMU 实现Cartographer 3D建图

1.创建backpack_2d_rs_16urdf文件

（路径：car2_ws/install_isolated/share/cartographer_ros/urdf)

  

<!-- 定义了一个名为cartographer_backpack_2d的机器人模型 -->  
<robot name="cartographer_backpack_2d">  
  
  <!-- 定义了一个名为orange的材料，其颜色为橙红色 -->  
  <material name="orange">  
    <color rgba="1.0 0.5 0.2 1" /> <!-- RGBA颜色值分别为红、绿、蓝、透明度，这里为不透明橙红色 -->  
  </material>  
  
  <!-- 定义了一个名为gray的材料，其颜色为深灰色 -->  
  <material name="gray">  
    <color rgba="0.2 0.2 0.2 1" /> <!-- RGBA颜色值分别为红、绿、蓝、透明度，这里为不透明深灰色 -->  
  </material>  
  
  <!-- 定义了一个名为imu_link的连杆，代表IMU（惯性测量单元） -->  
  <link name="imu_link">  
    <visual> <!-- 定义连杆的视觉属性 -->  
      <origin xyz="0 0 0" /> <!-- 视觉属性的原点位于连杆的坐标原点 -->  
      <geometry> <!-- 定义连杆的几何形状 -->  
        <box size="0.06 0.04 0.02" /> <!-- 连杆形状为盒子，尺寸为长0.06m, 宽0.04m, 高0.02m -->  
      </geometry>  
      <material name="orange" /> <!-- 使用前面定义的orange材料 -->  
    </visual>  
  </link>  
  
  <!-- 定义了一个名为rslidar的连杆，代表激光雷达 -->  
  <link name="rslidar">  
    <visual> <!-- 定义连杆的视觉属性 -->  
      <origin xyz="0 0 0" /> <!-- 视觉属性的原点位于连杆的坐标原点 -->  
      <geometry> <!-- 定义连杆的几何形状 -->  
        <cylinder length="0.05" radius="0.03" /> <!-- 连杆形状为圆柱体，长度为0.05m, 半径为0.03m -->  
      </geometry>  
      <material name="gray" /> <!-- 使用前面定义的gray材料 -->  
    </visual>  
  </link>  
  
  <!-- 定义了一个名为base_link的连杆，通常作为机器人其他部分的父连杆 -->  
  <link name="base_link" />  
  
  <!-- 定义了一个名为imu_link_joint的关节，类型为固定关节，连接imu_link和base_link -->  
  <joint name="imu_link_joint" type="fixed">  
    <parent link="base_link" /> <!-- 父连杆为base_link -->  
    <child link="imu_link" /> <!-- 子连杆为imu_link -->  
    <origin xyz="0 0 0" /> <!-- 关节的原点位于父连杆的坐标原点 -->  
  </joint>  
  
  <!-- 定义了一个名为laser_joint的关节，类型为固定关节，连接rslidar和base_link -->  
  <joint name="laser_joint" type="fixed">  
    <parent link="base_link" /> <!-- 父连杆为base_link -->  
    <child link="rslidar" /> <!-- 子连杆为rslidar -->  
    <origin xyz="0.1007 0 0.0558" /> <!-- 关节的原点相对于父连杆的位置，表示激光雷达在机器人上的安装位置 -->  
  </joint>  
  
</robot>

2.创建rs16_3d.lua文件

（路径：car2_ws/src/cartographer_ros/cartographer_ros/configuration_files)

-- 包含map_builder的配置文件  
include "map_builder.lua"  
-- 包含trajectory_builder的配置文件  
include "trajectory_builder.lua"  
  
-- 主配置选项  
options = {  
  -- 使用从map_builder.lua中定义的MAP_BUILDER配置  
  map_builder = MAP_BUILDER,  
  -- 使用从trajectory_builder.lua中定义的TRAJECTORY_BUILDER配置  
  trajectory_builder = TRAJECTORY_BUILDER,  
  -- 地图的全局参考框架  
  map_frame = "map",  
  -- 跟踪框架，通常设置为机器人的基座或主体框架  
  tracking_frame = "base_link",    
  -- 发布轨迹的参考框架，通常与tracking_frame相同  
  published_frame = "base_link",  
  -- 里程计数据的框架，用于机器人内部的里程计估计  
  odom_frame = "odom",  
  -- 是否由Cartographer提供odom_frame。如果外部系统已提供，则设置为false  
  provide_odom_frame = false,  
  -- 是否将发布的帧投影到2D平面  
  publish_frame_projected_to_2d = false,  
  -- 是否使用姿态外推器来预测机器人位置  
  use_pose_extrapolator = false,  
  -- 是否使用里程计数据  
  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,  
  -- 查找变换的超时时间（秒）  
  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数据的采样比率  
  imu_sampling_ratio = 1.,  
  -- 地标数据的采样比率  
  landmarks_sampling_ratio = 1.,  
}  
  
-- 3D轨迹构建器的特定配置  
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1  
  
-- 地图构建器的特定配置  
MAP_BUILDER.use_trajectory_builder_3d = true  -- 使用3D轨迹构建器  
MAP_BUILDER.num_background_threads = 7        -- 后台处理线程的数量  
  
-- 位姿图优化器的配置  
POSE_GRAPH.optimization_problem.huber_scale = 5e2  -- Huber损失的尺度参数  
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  -- Ceres求解器的最大迭代次数  
POSE_GRAPH.constraint_builder.min_score = 0.62  -- 约束构建的最小得分  
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66  -- 全局定位的最小得分  
  
-- 返回配置选项  
return options

3.创建cartographer_rs16_3d.launch文件

(路径：car2_ws/src/cartographer_ros/cartographer_ros/launch)

 

<launch>
  <param name="robot_description"
#对应之前的urdf文件
    textfile="$(find cartographer_ros)/urdf/backpack_2d_rs_16.urdf" />  
 
  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
 
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
#对应之前的lua文件
           -configuration_basename rs16_3d.lua"
      output="screen">
 
#对应自己的激光点云话题
    <remap from="points2" to="/rslidar_points" />
 
#对应自己的IMU话题
     <remap from="imu" to="/wit/imu" /> 
  </node>
 
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

4.创建demo_rs16_3d.launch文件

(路径：car2_ws/src/cartographer_ros/cartographer_ros/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" />
 
  <include file="$(find cartographer_ros)/launch/cartographer_rs16_3d.launch" />
 
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
</launch>

5.编译并运行 

catkin_make_isolated --install --use-ninja
source ./devel_isolated/setup.bash
roslaunch cartographer_ros demo_rs16_3d.launch