Cartographer（3）lidar+IMU建图

目录

一、准备工作

 二、IMU-SRC 使用 / 信息确认

目录

一、准备工作

 二、IMU-SRC 使用 / 信息确认

2.1 首先从官网/Github/Ros-wiki 中找到自己IMU所对应的SRC

2.2 下载后需查看的信息

 2.3 编译与使用

三、Lidar与IMU接入cartographer算法

3.1 确认 Lidar 与 IMU 的 topic 与 frame_id 信息

3.2 修改demo_revo_lds.launch文件

3.3 修改revo_lds.lua文件

3.4 修改backpack_2d.urdf文件（优先看3.4.2容易理解）

3.4.1 请参照 3.1 中确认的信息，同下方代码进行对照

3.4.2 关于 3.3.1 中 两个 origin xyz 的解释

3.4.2 URDF与坐标系的变换（2022.10.12更新）

四、cartographer 编译与运行

---

一、准备工作

        1-1 保证你系统的 cartographer 可以使用（Demo可以正常使用）

        1-2 保证系统你选择的 Lidar 可以正常使用（SRC与Cartographer可以进行实际建图）

1-1 准备工作请查看：SLAM学习之路系列

        十、Cartographer源码100%安装，rosdep问题100%解决（亲测）

1-2 准备工作请查看：SLAM算法实战（开源）系列

        四、Cargrapher单激光雷达建图（通用）

---

 二、IMU-SRC 使用 / 信息确认

        1- IMU 使用的请参考官网使用手册基本IMU-SRC大同小异，你需要确认和你IMU型号对应并能够在Ubuntu上使用

        2- 这里以TL740D为例，个人使用的为厂家提供，此处展示为 github 上找到的一个SRC 基本所有修改和确认的信息大同小异（未验证是否可用）

2.1 首先从官网/Github/Ros-wiki 中找到自己IMU所对应的SRC

        官网：请依据自己购买的 IMU 型号访问官网查找使用手册

        Github：I-Quotient-Robotics/tl740d_driver (github.com-未验证是否可用)

        ROS-wiki：Sensors - ROS Wiki （需要自己找对应的Sensors Src）

2.2 下载后需查看的信息

        IMU-SRC 目录结构如图：

        这里重点关注 README.md 文件，首先查看其中标定的依赖，可以进行提前安装。

        目前有串口、网口、RS232/485，对串口与RS232/485通讯与使用时需要打开串口权限，此处需要注意的是保证你的 Ubuntu 安装 serial 工具。

sudo apt install ros-<distro>-serial
========================================
因为我是melodic，所以将<distro>换为melodic
========================================
sudo apt install ros-melodic-serial

 2.3 编译与使用

        cd到所在的 Workspace 进行编译后使用

// 1-cd 到 当前的 workspace 运行以下命令完成编译
catkin_make

// 2-打开串口权限（具体串口命令跟使用的通讯口有关）
sudo chmod 777 /dev/ttyUSB*

// 3-启动当前IMU所对应的 SRC 
roslaunch -----------

        此时运行完成后，运行两个命令，查看 SRC 的数据 Topic 与 frame_id。

rostopic list
rostopic echo --noarr <topic-name>

此时请记录好当前 IMU 型号的 topic 与 frame_id() ：

        topic ：imu/data                                                frame_id : imu_link

---

三、Lidar与IMU接入cartographer算法

3.1 确认 Lidar 与 IMU 的 topic 与 frame_id 信息

        根据 2.3 中 IMU 的信息，与 四、Cargrapher单激光雷达建图（通用）2.2.1 Lidar 的信息：

	Topic	Frame_id
IMU	imu/data	imu_link
Lidar	/scan	laser

3.2 修改demo_revo_lds.launch文件

        打开 四、Cargrapher单激光雷达建图 中 2.2 中已经修改好的launch文件，可以使用Lidar进行建图的demo_revo_lds.launch。

        下方添加重映射信息，并根据 3.1 中表格的信息进行修改：

<launch>
<!--实际建图 time 不需要生成-->
  <param name="/use_sim_time" value="flase" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.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
          -configuration_basename revo_lds.lua"
      output="screen">
<!--映射lidar 中 topic 到 carto 中的 scan -->
<!--时间比较久不太记得了，如果出现error可以把后面的“/”去掉改为“scan”-->
    <remap from="scan" to="/scan" />
<!--

将下方的<imu_topic>改为 3.1 中已经确认好的 topic

-->
    <remap from="imu" to="<imu_topic>" />
  </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/demo_2d.rviz" />
<!--删除rosbag，由于添加注释会收到里面的两个横行干扰 干脆直接删除-->
</launch>

3.3 修改revo_lds.lua文件

        同 3.1 相仿，可以使用Lidar进行建图的revo_lds.lua，进行以下修改：

        1-修改 tracking_frame 为 3.1 中 imu_frame_id :imu_link

        2-修改 published_frame 为 3.1 中：base_link

        3-修改 provide_odom_frame = fslse            修改为 false 不发布位姿

        4-修改TRAJECTORY_BUILDER_2D.use_imu_data = true   启用IMU模块

include "map_builder.lua"
include "trajectory_builder.lua"
 
options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",           --修改为 imu_frame_id
  published_frame = "base_link",          --修改为 base_frame_id
  odom_frame = "odom",
  provide_odom_frame = false,            --修改为 flase 不发布位姿
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,       --此项为姿态外推器，建议为true，flase也可以
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  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 = 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.2           --修改为lidar_min_range
TRAJECTORY_BUILDER_2D.max_range = 12.           --修改为lidar_max_range
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true        --修改为true 开启使用imu模块
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

3.4 修改backpack_2d.urdf文件（优先看3.4.2容易理解）

        打开 ~/cartographer_ros/urdf/backpack_2d.urdf 文件，进行修改，这里与 tf 坐标发布有关系，后期发一个关于 urdf 与 tf 坐标发布的博客在 SLAM学习之路系列里（自己给自己挖坑）。

3.4.1 请参照 3.1 中确认的信息，同下方代码进行对照

        修改 <link name="imu_link"> 字段，将 imu_link 改为 3.1 中的 imu_frame_id

        修改 <link name="horizontal_laser_link"> 字段，将 horizontal_laser_link 改为 3.1 中的 lidar_frame_id

修改<!--joint 字段-->

       修改<joint name="imu_link_joint" type="fixed">中：

        child link 为 3.1 中的 imu_frame_id
        origin xyz 根据实际 与 scan 距离修改

       修改  <joint name="horizontal_laser_link_joint" type="fixed">中：

        child link 为 3.1 中  scan_frame_id
        origin xyz 根据实际 与 scan 距离修改

3.4.2 关于 3.3.1 中 两个 origin xyz 的解释

        这里跟 base_link 有关，具体解释写一个发布的博客在 SLAM学习之路系列里。

        此处以简图进行解释，仅用于 origin xyz 中的信息进行说明。（字丑勿喷）

         所有的计算都为 base_link 为原点进行计算，并以右手坐标系为基准，此处默认 IMU 与

Lidar 安装也遵循右手坐标系，且安装坐标系朝向一致。

        下方所有数值均为默认值，实际数值和修改处请根据个人情况进行计算，具体 urdf 文件的说明等更新。下方使用的 urdf 文件为 cartographer 的默认文件，我个人使用为自己编写的 urdf 文件，如果在使用中有问题，请查看： SLAM学习之路系列：十一、SLAM常用小功能（持续更新，碰到什么加什么）

对 urdf 文件进行检查并 rviz 查看是否存在错误。需要我个人使用文件的请私信。

<!--
  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_2d">
  <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 为 3.1 中的 imu_frame_id
修改 box size  为 IMU 型号实际外观尺寸
-->

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

<!--
修改 link name 为 3.1 中 lidar_frame_id
修改 cylinder size  为 Lidar 型号实际外观尺寸
-->

  <link name="horizontal_laser_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>


  <link name="base_link" />

<!-- joint 板块修改 -->
<!--
修改 child link 为 3.1 中的 imu_frame_id
修改 origin xyz 根据实际距离修改
-->

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

<!--
修改 child link 为 3.1 中 lidar_frame_id
修改 origin xyz 根据实际距离修改
-->

  <joint name="horizontal_laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="scan" />
    <origin xyz="0.1007 0 0.0558" />
  </joint>


</robot>

3.4.2 URDF与坐标系的变换 

        首先我们理解坐标系变换这个东西，实际建图中我们已知的最顶端的 tf 数为 /map，那么下方连接的可以为 /odom 与 /base_link，当你在  3.3中将 provide_odom_frame = false,改为true，则此时发布的 tf 为：

                                map ——> odom

        这时你就需要理解，现在使用的包括了 lidar 与 imu 他们两个之间的 tf 数是平级的，你要达成的最终 tf 树为：

map ——> odom(可选)——> base_footprint(可选) ——> base_link ——> sensor（底层部件）

        具体的关系与坐标转换可以自行搜索，坐标转换原理可以去看看被动变换与旋转矩阵

        URDF文件的作用可以简单理解为发布各部件的 tf 相对关系，我们可以将其拆分为3个部分，第一部分为 robot.URDF 中 robot的名称与其配置，即:

<robot name="cartographer_backpack_2d">
  <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>


<robot>

        其中robot name即你自己定义的robot名称，下方的两个material为配置的两种颜色，如果要自己改颜色的可以去搜索更改 rgba 这三个值（下方直接给传感器配置颜色时使用） 

        第二部分为机器人本体的参数配置，即：

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

        link name：为机器人实际存在的部件（设置footprint的也可以这么理解），此处一般为你传感器或机器人对应的frame_id（Tips：这块不对会抛ERROR） 

        origin xyz：部件自身的 tf 相对父级坐标系的位置关系，我们遵循右手坐标系原则，0 0 0即坐标系与父级坐标系重合

        box size：方型的 x y z，通俗理解长宽高

        material：颜色，取第一部分配置好的颜色即可

        第三部分为机器人各坐标系的相对配置，即：

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

        joint name：关节名称

        type：相当关系，fixed为固定，刚性连接无运动

        parent link：父级坐标系（framed id）

        child link：子坐标系（framed id）

        origin xyz：子坐标系相对父级坐标系的旋转关系，例如有的IMU安装后，IMU 的 x y实际上朝向并不是朝向车体正前方，我们就需要对z进行一定旋转（参考row pitch yall）

---

四、cartographer 编译与运行

        修改完上述操作后，对 cartographer 的工作空间进行编译运行：

=================编译工作空间==================
catkin_make_isolated --install --use-ninja

=============开发Lidar与IMU串口权限============
//ttyUSB* 与 ttyS* 需要自行判断

sudo chmod 777 /dev/ttyUSB*
sudo chmod 777 /dev/ttyS*

=================运行建图节点==================
roslaunch cartographer_ros demo_revo_lds.launch

---

        不积跬步无以至千里，不积小流无以成江河  --------------------------2：02