Cartographer をラズパイ4 単体で使う

はじめに

Google の自己位置推定パッケージ Cartographer がラズパイ４で単独で動きます。

PCで実行するよりは処理が重いですが、LD06 ミニ Lidar とモバイルバッテリと合わせ、小さい自己位置推定デバイスとなって地図が作れて面白かったです。

Cartographer は、Lidar だけあればSLAM 出来ちゃうところがとても面白いです。（オドメトリは有り・無しどちらでもOK）
(gmapping などはオドメトリ必須)

記事の前半分は、ラズパイ Ubuntu のセットアップについて
後ろ半分が、Cartographer ビルドについて、公式マニュアルに無い引っかかった所を補足して書いていますので、ご参考にどうぞ。

実行の様子

LD06 ミニ Lidar + モバイルバッテリ + ラズパイ４

環境

Raspberry pi 4　　　(PC でも同様だと思います。)
Ubuntu 20.04 server (20.04) + desktop 化
ROS (Noetic)
LD06 Lidar (2D　もちろん別のLidar でも良いので、その場合はLidar 部分を読み替えてセットアップしてください。)

Cartographer のチュートリアルでは catkin_make_isolated を使う事になっていますが、ディレクトリが別になって不便だったので、 catkin build でビルドします。

（なお Melodic だと Cartographer はビルドせず apt でインストールできるらしいです。今回は Noetic なのでビルドします。）

Ubuntu 環境セットアップ

Ubuntu 20.04 server + desktop化

Raspi Imager で Ubuntu 20.04 Server 64bit を選んでSDカードに書き込みました。

しかし、Ubuntu 20.04.5 バージョンだと、WiFi 5GHz が当環境ではおかしくて（故障？）、下記URLから 20.04.3 をダウンロードして書き込みました。

Ubuntu 20.04 server の デスクトップGUI化は、以下を参考にしました。

このときはLAN ケーブルでネットに接続し、GUI 化したあとに WiFi 接続しました。
（コマンドラインからでも WiFi 接続の設定は出来ると思いますが面倒なので）

ID: ubuntu
pass: ubuntu
# 新しいパスワード実行

# ログインできない場合は、なにやら画面１ページ分のログが出たあとじゃないとログインできない？

sudo apt update
sudo apt install ubuntu-desktop

git clone https://github.com/wimpysworld/desktopify.git
# 英語キーボード設定になっていると、Shift + [;]が : コロンになりました
cd desktopify
sudo ./desktopify --de ubuntu --oem          #--oem オプションをつけて、初期設定できるようにしてる

sudo reboot

スワップ設定

しなくても大丈夫かもしれませんが、Cartographer ビルドする時にメモリが足りなくなってフリーズしないよう、一応スワップメモリを設定しておきましょう。

参考

sudo swapon --show
sudo fallocate -l 4G /swapfile
sudo chmod 600 /swapfile
sudo mkswap /swapfile
sudo swapon /swapfile
sudo swapon --show

echo '/swapfile swap swap defaults 0 0' | sudo tee -a /etc/fstab
sudo reboot now
sudo swapon --show

ROS (Noetic) インストール

公式の通り

前述のように、便利な catkin build システムを使おうと思います。
参考：

Lidar (LD06 Lidar) ドライバパッケージ　インストール

使うLidar のパッケージ

catkin_ws/src/ld06_lidar/launch 　内の /dev/ttyUSB0 等設定を合わせましょう。
Ubuntuはよくシリアル通信デバイスの権限エラーが出るので、 chmod 777 /dev/ttyUSB0 、またはdialout グループへのユーザー追加などググってみてください。
動いてるかどうかは、 rostopic echo /scan でデータ受信できていれば、数字の羅列が表示されます。

Cartographer のビルド

Cartographer のラズパイ上でのビルドはそれなりに時間がかかります。（60分～？）
手順自体は簡単です。

基本的には公式チュートリアルの説明と同じですが、catkin build 式に変えた所、修正が必要だったところを変更したものを以下に記します。

sudo apt update
sudo apt install -y python3-wstool python3-rosdep ninja-build stow
sudo apt install python3-catkin-tools

mkdir -p ~/catkin_ws
cd ~/catkin_ws
mkdir -p ./src
cd ~/catkin_ws
catkin build
source devel/setup.bash

wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

#sudo rosdep init      # もし ROS(Noetic) インストール時に行ってたらこれは不要
rosdep update

src/cartographer/scripts/install_abseil.sh

package.xml の abseil について修正

gedit ./src/cartographer/package.xml 

46 行目の libabsl-dev の行をコメントアウト（ で囲む）

abseil を手動でインストールするためか、うまく認識されないため。

関連 issue (2022年現在、最近発生した現象？)

python コマンド実行バージョンの修正

また、次の catkin build でエラーが出ないように下記設定をします。
Python3 でなく Python2 が途中で使われてしまってるためのエラーです。

以下を参考にして、python とコマンド実行した際に 2.x 系でなく 3.x 系で実行されるようにして catkin build しましょう。

ls /usr/bin/ | grep python 
which update-alternatives
sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.8 1
sudo update-alternatives --install /usr/bin/python python /usr/bin/python2.7 2

sudo update-alternatives --config python          #sudo
   --> 2 (python 3.8 の方) を選ぶ

python --version

続けます。

rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
sudo apt remove ros-${ROS_DISTRO}-abseil-cpp

catkin build

ビルドできました！

（2022-10-25 追記）
●PC Ubuntu 20.04 でトライした時に、このあとのcatkin build でこんなエラーが出ました。

ImportError: cannot import name ‘soft_unicode’ from ‘markupsafe’

こちらを参考に、

pip3 install markupsafe==2.0.1

として、ビルドエラーが消えました。バージョン更新による不具合でしょうか。

参考：

●また、GMOCK_LIBRARY 変数が無いよというエラーの時は、

sudo apt install -y libgmock-dev

としました。

設定ファイル各種

ここは、ひとまず自前のLidar で動いた状態の設定ファイルを置いておきます。
折りたたんであるのをクリックして表示してください。

共有フォルダにファイルも置いておきますのでダウンロードして使用してください。

Cartographer_ros の各ディレクトリに入れます。
（catkin_make_isolated でビルドしたなら catkin_ws\install_isolated\share\cartographer_ros）
（catkin build でビルドしたなら catkin_ws\src/cartographer_ros）

パラメータの説明は、別記事に書いてます。

launch ファイル（起動ノードなどの設定）

launch/myrobot2d.launch

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/myrobot.urdf" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_robot2d.lua"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

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

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />


  <node name="rviz" pkg="rviz" type="rviz" required="true"
    args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />


</launch>

launch/myrobot2d_localization.launch

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/myrobot.urdf" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename myrobot2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

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

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />


  <node name="rviz" pkg="rviz" type="rviz" required="true"
    args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

</launch>

lua ファイル（Cartographer のパラメータ設定）

configuration_files/myrobot2d.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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.num_accumulated_range_data = 10

TRAJECTORY_BUILDER_2D.use_imu_data = false

return options

myrobot2d_edit.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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.num_accumulated_range_data = 10

TRAJECTORY_BUILDER_2D.use_imu_data = false


--added

TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 20.                     -- 自分の Lidar の性能に合わせる。 LD06 Lidar なら 12m + α
-- TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5      --'max_range' を超える点は、この長さで空白部として挿入されます。

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true    -- 相関スキャンマッチャーというのを有効にします。
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1                 -- 1 (歪み対策。増やすとレスポンス低下したので 1 or 2?)

-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 50.
-- TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100

-- TRAJECTORY_BUILDER_2D.use_imu_data = true  （既に前で false に設定済）
-- TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.

-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10.
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40.

-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.use_nonmonotonic_steps = false
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 1

-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1e-1
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

-- TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.
-- TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
-- TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.)

-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90             -- 後で120 にする

-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.grid_type = "PROBABILITY_GRID"    -- or "TSDF"

-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D"
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.insert_free_space = true
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49

-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05
-- TRAJECTORY_BUILDER_3D.submaps.high_resolution = 0.10
-- TRAJECTORY_BUILDER_3D.submaps.low_resolution = 0.45
-- TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter.max_range = 15.
-- TRAJECTORY_BUILDER_3D.low_resolution_adaptive_voxel_filter.max_range = 60.

-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.truncation_distance = 0.3
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.maximum_weight = 10.
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_free_space = false
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.num_normal_samples = 4
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.normal_estimation_options.sample_radius = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.project_sdf_distance_to_scan_normal = true
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_range_exponent = 0
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5
-- TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.tsdf_range_data_inserter.update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5

-- POSE_GRAPH.optimize_every_n_nodes = 90

-- POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 7.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)

-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3

-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 7
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.branch_and_bound_depth = 8
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.full_resolution_depth = 3

-- POSE_GRAPH.constraint_builder.min_score = 0.55

-- POSE_GRAPH.constraint_builder.loop_closure_translation_weight = 1.1e4
-- POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 1e5
-- POSE_GRAPH.matcher_translation_weight = 5e2
-- POSE_GRAPH.matcher_rotation_weight = 1.6e3
-- POSE_GRAPH.optimization_problem.*_weight
-- POSE_GRAPH.optimization_problem.ceres_solver_options

-- POSE_GRAPH.optimization_problem.log_solver_summary = false
-- POSE_GRAPH.optimization_problem.use_online_imu_extrinsics_in_3d = true

-- POSE_GRAPH.optimization_problem.huber_scale = 1e1

-- POSE_GRAPH.max_num_final_iterations = 200




-- POSE_GRAPH.optimize_every_n_nodes = 0    -- 90 -> 0  Global SLAM を OFF にする

-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2  -- 10 -> 200
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2     -- 40 -> 400 

-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025                    -- 増やす デフォルト： 0.025
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05    -- 増やす デフォルト： 0.05
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200   -- 小さく デフォルト： 200
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.        -- 小さく デフォルト： 50.
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5       -- 増やす デフォルト： 0.5
-- TRAJECTORY_BUILDER_2D.max_range = 30.                              -- 小さく デフォルト： 30.
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20                  -- 小さく デフォルト： 20


-- POSE_GRAPH.optimize_every_n_nodes = 90                             -- 小さく デフォルト： 90
-- MAP_BUILDER.num_background_threads = 4                             -- 増やす(CPUコア数)デフォルト: 4
-- POSE_GRAPH.global_sampling_ratio = 0.003                           -- 小さく デフォルト： 0.003
-- POSE_GRAPH.constraint_builder.sampling_ratio = 0.3                 -- 小さく デフォルト： 0.3
-- POSE_GRAPH.constraint_builder.min_score = 0.55                     -- 増やす デフォルト:  0.55

-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200   -- 小さく デフォルト： 200
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.        -- 小さく デフォルト： 50.
-- TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5       -- 増やす デフォルト： 0.5
-- TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025                    -- 増やす デフォルト： 0.025
-- TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.05    -- 増やす デフォルト： 0.05
-- TRAJECTORY_BUILDER_2D.submaps.num_range_data = 20                  -- 小さく デフォルト： 20

-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy_search_window = 5.
                                                                      -- 小さく デフォルト： 5.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_z_search_window = 1.
                                                                      -- 小さく デフォルト： 1.
-- POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(30.)
                                                                      -- 小さく デフォルト： math.rad(30.)
-- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(20.)
                                                                      -- 小さく デフォルト： math.rad(20.)
-- POSE_GRAPH.global_constraint_search_after_n_seconds = 10.          -- 増やす デフォルト:  10.
-- POSE_GRAPH.constraint_builder.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 10
                                                                      -- 小さく デフォルト： 10
-- POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 50
                                                                      -- 小さく デフォルト： 50
-- TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
                                                                      -- 小さく デフォルト： 20


-- from Mr.PINTO03091 's settings. Thanks!

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
                                                                   -- デフォルト: 0.1 -> 0.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
                                                                   -- デフォルト: math.rad(20.) -> math.rad(35.)
POSE_GRAPH.constraint_builder.min_score = 0.65                     -- デフォルト: 0.55 -> 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7  -- デフォルト: 0.6 -> 0.7
POSE_GRAPH.optimization_problem.huber_scale = 1e3                  -- デフォルト: 1e1 -> 1e3
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
                                                                   -- デフォルト: 1.  -> 10
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120                 -- デフォルト: 90  -> 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1      -- デフォルト: 0.2 -> 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)
                                                                   -- デフォルト: math.rad(1.) -> math.rad(0.2)


return options

（→　次の記事にまとめてますが、こちらの myrobot2d_edit.lua の内容のパラメータでレスポンス・品質がアップしていますので、この内容を myrobot2d.lua に書き込んで違いを見るのも良いと思います。）

configuration_files/myrobot2d_localization.lua

include "myrobot2d.lua"


TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 3

return options

URDF ファイル（ロボット構成ファイル）

urdf/myrobot.urdf

<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="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="laser">
    <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 name="imu_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />
    <origin rpy="0 0 0"  xyz="0 0 0" />
  </joint>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin rpy="0 0 0"  xyz="0.1007 0 0.0558" />
  </joint>


</robot>

使い方

今回は catkin build でビルドしたので、Cartographer のチュートリアルにある

source install_isolated/setup.bash

ではなく、

source devel/setup.bash

を、パッケージの実行などの前に行います。

そして以下が、cartographer 実行コマンドです。

# ターミナル１
cd ~/catkin_ws
source devel/setup.bash
roslaunch ld06_lidar ld06.launch 

# ターミナル2  [Ctrl] + [Shift] + T でターミナルのタブ追加できる
source devel/setup.bash
roslaunch cartographer_ros myrobot2d.launch

# ターミナル3 (pgm, yaml, 等Cartographer 以外で地図を利用するデータを保存する場合。いらなければ不要)
source devel/setup.bash
rosrun map_server map_saver

# ターミナル4 (Cartographer 用の pbstream 形式で地図を保存する場合)
source devel/setup.bash
rosservice call /finish_trajectory 0
rosservice call /write_state "{filename: '${HOME}/my_mapdata.pbstream', include_unfinished_submaps: "true"}"

# ターミナル5 (ターミナル2,3,4 を終了させたあとに、Localization 位置推定で実行する場合)
source devel/setup.bash
roslaunch cartographer_ros myrobot2d_localization.launch load_state_filename:=${HOME}/my_mapdata.pbstream

Google 公式サンプル

なお、こちらの公式チュートリアルを行えば、あらかじめ用意されている rosbag データや各種設定ファイルにて、 Cartographer の実演デモを簡単に実行する事ができます。（たぶんこのページより先に、そのチュートリアルをやるべきですw）

地図がどんどん生成されるのを見たり、ループクローズの様子が見られるのは楽しいｗ

補足

rviz で表示するものを減らすと fps 改善します。（特に tf ?）　地図生成の初期段階では 30fps も可能でした。

resolution を大きくすると軽くなります。Tuning は公式での解説があったりします。そのあたりを続く記事で解説しています。

Lidar のlaunch と Cartographer の launch を一緒にすれば、一発起動できて便利ですね

ほか、ファイル共有のsamba や、VNC リモート操作をモニタレスでできるよう設定すると、他PCからモニタリングできて便利＆吉です。

デフォルトの launch ファイルの中には、rosbag を読み込んで地図を生成する demo_backpack_2d.launch ファイルなどもありますので参考にしてみてください。
(パラメータ調整する時に有用)

なお、ROS_MASTER_URI 等の設定を変えて ~/.bashrc に書いて source コマンドすれば、Cartographer は PC にインストールし、その処理をROS ホストに実行させ、センサだけラズパイということも出来ますので、それも楽しいです。

もちろん、UMPC などモバイルPCで実行するのも処理性能的に有利でサクサクで楽しいです。

export ROS_MASTER_URI=http://192.168.1.7:11311
export ROS_HOSTNAME=192.168.1.4

さいごに

試行錯誤の全てを書いたツイートリンクも置いておきます。

関連記事

参考：

パラメータ等、自前のロボットで動いた状態のパラメータファイルがそのまま利用できたのは心強かったです！
（最初動かないな～と試行錯誤して悩んでたため）

公式パラメータガイド

（↑　下の方に ceres solver のオプションの説明があります）

参考になるリンク

Cartographer ROS Documentation

(2D)

(2D / 3D)