cartographer跑自己的數據+3d建圖篇(Lidar+imu)

接上篇2d建圖(那個,參數解釋,網上太多了,我真的懶得寫了,大家自己去官網查吧,最近好忙)
老規矩,按步走,做好筆記防止以後忘記!!!(步驟和2d的差不多,只是配置不同)

(鐳神雷達沒有反射率這個數據麼?建圖過程中一直在報警告,找不到反射率什麼的!!)
硬件:鐳神32線半固態雷達,導遠慣導系統.
軟件:Ubuntu1604,ROS-Kinetic,cartographer框架
(建議:改動文件建議自己複製源文件進行新建文件改動，源文件作爲保留用。)

第一步,確定硬件之間的座標轉換關係.
根據上面所說,我用到了一個多線雷達和IMU,所以在跑數據時必須要有這兩個傳感器的TF樹變換關係.在cartographer中,用的是URDF文件進行構建該關係,當然,後來我在實車測試時屏蔽了這個文件,直接用車上的tf變換關係也是可以的,只要加入imu到激光雷達的變化即可.cartographer的urdf文件定義在~/catkin_cartographer/install_isolated/share/cartographer_ros/urdf$中,名字自己要記住,
如下我的urdf文件:

<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>
        <cylinder length="0.07" radius="0.05"/>
      </geometry>
    </visual>
  </link>

  <link name="lidar_mid">
    <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="lidar_left">
    <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="lidar_base">
    <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="lidar_rear">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

    <joint name="imu2mid" type="fixed">
    <parent link="lidar_mid"/>
    <child link="imu" />
    <origin xyz="0 -4.0 -0.2" rpy="0 3.14 0"/>
  </joint>

  <joint name="left2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_left" />
    <origin xyz="-0.85 -0.42 0" rpy="0 0.03 0" />
  </joint>

  <joint name="base2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_base" />
    <origin xyz="0.85 -0.42 0" rpy="0 0 -1.57" />
  </joint>

  <joint name="rear2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_rear" />
    <origin xyz="0 -4.6 0" rpy="-3.14159 -3.14159 0" />
  </joint>


</robot>

要強調的是,裏面的參數都是多雷達標定後得到的,要根據實際中你車上的座標去改.

2.第二步,準備數據,這個不麻煩,自己準備3d數據包即可,實在找不到的,博主這裏附上自己的3d數據包,自行下載.

鏈接: https://pan.baidu.com/s/1Hu1qd4SQ2KKGqXbUU6rMxA 提取碼: 4asr

第三步,配置文件條理,這個步驟其實不復雜,只是每在配置文件夾裏改一次,就要去源文件夾裏做一下相同的修改.先貼路徑,
配置文件夾路徑:

源文件夾路徑:

~/catkin_cartographer/src/cartographer_ros/cartographer_ros/

這兩個文件夾裏最主要的東西都是一樣的,configuration_files是存儲.lua的文件夾,launch是存儲啓動命令的文件夾,urdf是存儲小車模型的文件夾.
**注意:**配置文件夾裏的urdf決定了你的小車的模型,即TF樹,源文件夾裏的urdf你可以不用管.
而urdf文件在上面有說過了.

第四步,開始配置:
首先,進入配置文件夾裏的configuration_files文件夾,新建my_robot_3d.lua文件(你可以複製原本的backpack_3d.lua,然後修改內容).我的文件內容如下:

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu",
  published_frame = "lidar_mid",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,
  use_odometry = false,
  use_nav_sat = false,#這裏選擇是否用GPS,博主的圖建出來效果不錯,定位也可以,後面也嘗試了加入GPS建圖定位,很nice
  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_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

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

保存退出,進入同級目錄的launch文件夾下,新建demo_my_robot_3d.launch文件(可複製backpack_3d.launch,然後修改),內容如下

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/minibus.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 my_robot_3d.lua"#加載3d的配置文件
      output="screen">
    <remap from="points2" to="/minibus/front/lslidar_point_cloud" />#這裏是你自己3d點雲雷達的話題
    <!--<remap from="points2_2" to="vertical_laser_3d" />-->
  </node>

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

然後,在該文件夾裏找到demo_backpack_3d.launch文件,用gedit打開修改成:

<launch>
  <param name="/use_sim_time" value="false" />

  <include file="$(find cartographer_ros)/launch/demo_my_robot_3d.launch" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

保存退出到同級目錄中,進入urdf文件夾,檢查minibus.urdf文件是否存在,且裏面座標變換關係是否正確.至此,配置文件夾裏的改動結束.

第五步, 配置源文件夾裏的文件.
步驟按照配置文件夾裏的配置步驟來,懶的話,直接複製過去也是可以的.
第六步, 檢查兩個文件夾裏做了相同的配置之後,保存退出到工作空間路徑:
(不要問我源文件夾和配置文件夾是什麼,上面已經講過了,往上翻翻就知道了)

cd ~
cd ~/catkin_cartographer/
catkin_make_isolated --install --use-ninja
source devel_isolated/setup.bash

嘗試建圖
命令:

cd ~/catkin_cartographer/
source devel_isolated/setup.bash
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=~/2d.bag

效果如下,其實,我懶得重新建圖了,下面這是我跑純定位模式時的地圖,紅色點雲部分就是點雲匹配上後給出的效果.

保存地圖
這裏統一默認將地圖保存爲.pbstream的格式，當你覺得圖建的不錯時候，另起終端，輸入如下兩條命令。路徑0是cartographer默認的建圖ID

cd catkin_cartographer/
source devel_isolated/setup.bash
rosservice call /finish_trajectory 0 //結束路徑0 的建圖
rosservice call /write_state "{filename: '${HOME}/Downloads/mymap.pbstream'}" //保存地圖

至此，完工，建好的圖也被保存在了Home/Download/路徑下，名字爲mymap.pbstream。

cartographer跑自己的數據+3d建圖篇(Lidar+imu)

DAPPER 事務 TRANSACTION

cartographer安裝過程記錄

cartographer跑自己的數據包-2d建圖篇（laser+Imu建圖篇）

cartographer跑自己的數據+3d建圖篇(Lidar+imu)

Velodyne 16線激光雷達-多雷達版本

cartographer基於3d地圖的純定位模式

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結