地図の作成とナビゲーション

シミュレータを用いてナビゲーションを体験します．
Tags:
Categories:

目的

自律移動ロボットが要求される技術の一つに指定した位置へ移動するナビゲーション機能あります． ナビゲーションを実現するためには，ロボット自身が地図上のどこに存在するかを推定し，障害物を回避しながら最適な経路を計画，そして経路通りにロボットを走行させる必要があります． ROSではナビゲーション関連のパッケージが用意されているため，簡単に試すことができます． 今回はロボットシミュレータstageを用いてナビゲーションを体験します．

準備

必要なパッケージを事前にインストールしておきます．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21

$ sudo apt update
$ sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-nav2-map-server
$ sudo apt install ros-humble-bondcpp
$ sudo apt install libsuitesparse-dev

$ mkdir tmp
$ cd tmp
$ wget http://ceres-solver.org/ceres-solver-2.2.0.tar.gz
$ sudo apt-get install cmake
$ sudo apt-get install libgoogle-glog-dev libgflags-dev
$ sudo apt-get install libatlas-base-dev
$ sudo apt-get install libeigen3-dev
$ sudo apt-get install libsuitesparse-dev

$ tar zxf ceres-solver-2.2.0.tar.gz
$ mkdir ceres-bin
$ cd ceres-bin
$ cmake ../ceres-solver-2.2.0
$ make -j3
$ make test
$ sudo make install

ロボットシミュレータSTAGEの起動

以前にgithubからクローンしたstageシミュレータを使用します． 下記のコマンドにより正常に起動するか確認しましょう．

1
2
3

$ cd stage_ws
$ source install/setup.bash 
$ ros2 launch stage_ros2 stage.launch.py world:=cave

もしstageシミュレータが起動しない場合には，こちらのページを参考にして改めてクローンしてください． 次にデータ可視化ツールrviz2を起動します． runコマンドでrviz2を起動して可視化したい項目を手動で設定しても良いですが，今回はstageシミュレータ側で用意されているlaunchファイルを使ってrviz2を起動します． 設定ファイルを読み込んでrviz2を起動できるため，起動時に幾つかの項目が設定された状態で立ち上がります．

1
2
3

$ cd stage_ws
$ source install/setup.bash
$ ros2 launch stage_ros2 rviz.launch.py config:=cave

地図作成

まずは地図を作成します． ROS2では地図を作成するパッケージが幾つか公開されていますが，今回はslam_toolboxパッケージを使用します． aptを用いてインストールしましょう．

1

$ sudo apt-get install ros-humble-slam-toolbox

stageシミュレータを使用する場合，デフォルトのままでは正常に起動しないので設定ファイルを編集する必要があります． (多くのシミュレータでは，設定ファイルを編集しないでも動作することが多いです．) 必要なファイルをstage用にコピーします．

1
2

$ sudo cp /opt/ros/humble/share/slam_toolbox/launch/online_async_launch.py /opt/ros/humble/share/slam_toolbox/launch/online_async_launch_stage.py
$ sudo cp /opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async.yaml /opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async_stage.yaml

先ほどコピーしたファイルを編集します． まずはlaunchファイルを編集します． ハイライトされている21行目を下記のように編集してください．

1

$ sudo vim /opt/ros/humble/share/slam_toolbox/launch/online_async_launch_stage.py

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_params_file = LaunchConfiguration('slam_params_file')

    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation/Gazebo clock')
    declare_slam_params_file_cmd = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(get_package_share_directory("slam_toolbox"),
                                   'config', 'mapper_params_online_async_stage.yaml'),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node')

    start_async_slam_toolbox_node = Node(
        parameters=[
          slam_params_file,
          {'use_sim_time': use_sim_time}
        ],
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen')

    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time_argument)
    ld.add_action(declare_slam_params_file_cmd)
    ld.add_action(start_async_slam_toolbox_node)

    return ld

1

$ sudo vim /opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async_stage.yaml

次に設定ファイルを編集します． ハイライトされている15，16行目を下記のように編集してください．

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

slam_toolbox:
  ros__parameters:

    # Plugin params
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /base_scan
    use_map_saver: true
    mode: mapping #localization

    # if you'd like to immediately start continuing a map at a given pose
    # or at the dock, but they are mutually exclusive, if pose is given
    # will use pose
    #map_file_name: test_steve
    # map_start_pose: [0.0, 0.0, 0.0]
    #map_start_at_dock: true

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.05
    max_laser_range: 20.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.5
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true 
    loop_match_minimum_chain_size: 10           
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

地図作成ノードを起動しましょう．

1

$ ros2 launch slam_toolbox online_async_launch_stage.py

rviz2で作成した地図を可視化するためにrviz2の設定を幾つか変更する必要があります． 1つ目は基準となるフレームの設定です． DisplaysのGlobal OptionsにあるFixed Frameをmapに変更しましょう． 2つ目は作成した地図の可視化です． Displaysの左下にあるAddボタンをクリックし，可視化したい項目のメニューを表示します． 項目の中からMapを選びOKをクリックしてください． DisplaysにMapが追加されるのでダブルクリックし，Topicにて/mapを入力します． 上記の2つを設定すると画面中央にて作成された地図が表示されます．

ロボットに搭載されたカメラで観測した範囲の地図を作成します． 下記のコマンドでキーボードによりロボットを操作して地図を作成しましょう． その際，ロボットが障害物に衝突するとズレが生じて不完全な地図になるので注意しましょう．

1

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard

下記は地図を作成した例です．

作成した環境地図の例

ナビゲーション実行時に作成した地図を使用します． 地図が完成したら保存しましょう．

1
2
3

$ cd ~/stage_ws/src/stage_ros2/
$ mkdir map
$ ros2 run nav2_map_server map_saver_cli -f ~/stage_ws/src/stage_ros2/map/map

上記のコマンドを実行すると，map.yamlとmap.pgmというファイルが作成されます． map.yamlはパラメータ，map.pgmは画像形式で保存した地図です． ここまでの作業が完了したら起動しているターミナルを全て閉じましょう．

ナビゲーション

作成した地図を用いてナビゲーションを行います． stageシミュレータの仕様上，パラメータファイルを編集しなければ動きません． アンダーレイ(/opt/ros/以下にあるROS2のコアシステム)の編集は避けたいですが，下記以外の方法では手間がかかるので必要最低限の編集に留めます． パラメータファイルをコピーしてエディタで開きます．

1
2
3

$ cd /opt/ros/humble/share/nav2_bringup/params
$ sudo cp nav2_params.yaml nav2_params_stage.yaml
$ sudo vim nav2_params_stage.yaml

211行目を下記のように編集してください．

211

          topic: /base_scan

上記の編集が完了したら，stageシミュレーターを起動します．

1

$ ros2 launch stage_ros2 stage.launch.py world:=cave

次にナビゲーションに必要なノードを起動します． 下記を実行する際に，シミュレーターの使用の有無を表すuse_sim_timeをTrueに， 先ほど作成した地図の絶対パスをmapで与えます． (下記をそのまま実行しても正常に起動できません，適宜必要な箇所を変更してください．) また，params_fileにて先ほど編集したパラメータファイルを絶対パスで与えます．

1

$ ros2 launch nav2_bringup bringup_launch.py  use_sim_time:=True  map:=/home/yuu/stage_ws/src/stage_ros2/map/map.yaml params_file:=/opt/ros/humble/share/nav2_bringup/params/nav2_params_stage.yaml

stageシミュレータはtfによる座標系を扱っていないため，このままではナビゲーションを実行できません． static_transform_publisherにより，フレームodomとワールド座標系を表すmapを繋ぎます．

1

$ ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --roll 0 --pitch 0 --yaw 0 --frame-id map --child-frame-id odom

ここまでの作業が完了したらデータ可視化ツールrviz2を起動してください．

1

$ ros2 launch stage_ros2 rviz.launch.py config:=cave

rviz2の起動後，Displaysの左下にあるAddボタンをクリックし，可視化したい項目のメニューを表示します． 項目の中からMapを選びOKをクリックしてください． DisplaysにMapが追加されるのでダブルクリックし，Topicにて/mapを入力します． 作成した地図を表示する設定が完了しました(まだ地図は表示されません)． 下記のコマンドを実行して地図を読み込みます． map_urlにて地図の絶対パスを与えます． (下記をそのまま実行しても正常に起動できません，適宜必要な箇所を変更してください．)

1

$ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /home/yuu/stage_ws/src/stage_ros2/map/map.yaml}"

上記を実行するとrviz2にて作成した地図が表示されます． (表示されない場合には何かミスをしています．地図作成後の内容をやり直してください．)

これでナビゲーションを行う準備が整いました． rviz2から目的地を与えてみましょう． rviz2の画面上部に2D Goal Poseというボタンがあります． このボタンにより目標位置・姿勢を設定することができます． 目標とする位置でクリックし，そのままドラッグすると矢印が回転するので姿勢を設定できます．

下記の動画はナビゲーションの例です．