ROS 2においてNavigation2を用いた自動走行を行う方法

# はじめに
　今回も引き続き、ROS 2において、対向二輪型ロボットに自動走行を行わせる方法について記載します。今回の記事では、前回作成した環境地図と、Navigation2を用いて、ロボットに自動走行を行わせていきます。
　使用するロボットや環境、地図については、前回のものを流用しています。

::: note
この記事は[ROSに関する投稿](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d)の一部です。
目次は[こちら](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d#%E7%9B%AE%E6%AC%A1)
:::

# 前提条件
　今回の記事は以下の環境で動かすこと前提に記載しています。
||条件|
|---|---|
|OS|Ubuntu 22.04|
|ROS|ROS 2 humble|

# Navigation2を用いた自動走行
　ROS 2には、目的地を設定すると、現在のロボットの位置から目的地までの経路を描き、ロボットを自動的に目的地まで走行させる**Navigation2**というパッケージがあります。このNavigation2では、LiDARと事前に作成した環境地図を使用して、ロボットに自動走行を行わせることができます。

### Navigation2の仕組み
　まず、Navigation2の仕組みについて、図を用いて簡単に説明します。

　Navigation2では、Behavior Tree(BT)を使用してロボットの行動を管理します。BTは、ロボットやゲームAIなどの動作を制御するための手法であり、行動や判断を表すツリー構造の図を用いることで複雑な動作を実現します。Navigation2のBTでは、ロボットが目的地に到達するまでの一連の動作が定義されており、状況に応じて適切な行動を選択するために使用されます。
|Navigation2のデフォルトのBehavior Tree|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/aa05829f-763d-9328-2e93-a5b1760c389d.png">|
|参照: [Detailed Behavior Tree Walkthrough — Nav2 1.0.0 documentation](https://docs.nav2.org/behavior_trees/overview/detailed_behavior_tree_walkthrough.html)|
 
　BT内の具体的な行動を実行するために、Navigation2では4つのアクションサーバー、Planner、Controller、Behavior、Smootherが用意されています。Plannerは目的地までの経路を計算し、Controllerはその経路をロボットが辿るための制御を行います。Behaviorはロボットが動けなくなった場合などの障害が発生したときに回復動作を行うために使用され、Smootherは必要に応じてPlannerによって求められた経路を最適化します。
|Navigation2の構造|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/4402a0d2-ecae-229b-622c-e6fed86088ae.png">|
|参照: [Nav2 — Nav2 1.0.0 documentation](https://docs.nav2.org/)|

　また、環境を認識するために**コストマップ**(Cost map)という表現が使われます。コストマップは、不明(unknown)、空き(free)、占有(occupied)、膨張コスト(inflated cost)を含む領域で構成された2次元地図です。このコストマップは、経路を計算したり、ロボットの制御を行ったりするために使用されます。コストマップは複数の層で構成されており、環境地図から生成される層(static_layer)やLiDARなどのセンサから生成される層(obstacle_layer)の他、独自の層を追加することも可能です。
　さらに、フィルタを適用することで、マップの特定の位置で特定のアクションを発生させることができ、例えば、ロボットが侵入できない立ち入り禁止ゾーンや速度制限エリアなどを設定することができます。
|Navigation2のデフォルトのコストマップ|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/c3b21179-3131-61f4-c695-624d62137ad8.png">|

　SLAMと同様、Navigation2での自己位置推定にはオドメトリなどが使用されますが、Navigation2ではオドメトリの誤差を吸収するために、Adaptive Monte Carlo Localization(**AMCL**)などの手法が使用されます。AMCLでは、パーティクルフィルタ(Particle Filter)という手法を用いて、環境地図を基にロボットの現在位置を確率的に推定します。まず、パーティクル(Particle)と呼ばれる仮想的な点を地図上に複数生成します。これらのパーティクルは、ロボットの現在位置の可能性がある場所を示しています。次に、各パーティクルは、LiDARなどのセンサから得られる情報と地図情報を比較し、どれだけ正確に位置を表しているか(尤度)に基づいて重みが付けられます。この重み付けの結果に基づいて、より正確な位置にパーティクルを移動させていくことで、位置推定の精度を向上させていきます。
|AMCL|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/faaaea9c-0999-8c54-96e6-1160dc4318db.png">|

### Navigation2の実装
　ここからは、[前回作成した地図](https://qiita.com/kccs_mitsuhiro-teraoka/items/253623296504a95fab88)を基に、実際にNavigation2を使用して、ロボットに自動走行を行わせていきます。

#### Navigation2関連パッケージのインストール
　まずNavigation2関連パッケージのインストールを行います。以下のコマンドを実行してください。

```
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
```

#### Navigation2の設定ファイルの用意
　Navigation2の設定ファイルを用意します。デフォルトの設定ファイルは[nav2_params.yaml](https://github.com/ros-navigation/navigation2/blob/humble/nav2_bringup/params/nav2_params.yaml)です。デフォルトではbase_frame_idが**base_footprint**に設定されているため、以下のyamlファイルを作成し~/ros2_ws/src/sim_py_01/configに配置します。今回、ファイル名はnavigation_params.yamlとしています。

```~/ros2_ws/src/sim_py_01/config/navigation_params.yaml
amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.01
    alpha2: 0.01
    alpha3: 0.01
    alpha4: 0.01
    alpha5: 0.0
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 10.0
    laser_min_range: 0.05
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.1
    update_min_d: 0.15
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
  ros__parameters:
    use_sim_time: True
bt_navigator_navigate_to_pose_rclcpp_node:
  ros__parameters:
    use_sim_time: True
controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]
    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.2
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.1
      min_speed_theta: 0.0
      acc_lim_x: 1.0
      acc_lim_y: 0.0
      acc_lim_theta: 1.6
      decel_lim_x: -1.0
      decel_lim_y: 0.0
      decel_lim_theta: -1.6
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.05
      xy_goal_tolerance: 0.01
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      footprint: "[[0.1, 0.2], [0.1, -0.2], [-0.1, -0.2], [-0.3, -0.15], [-0.3, 0.15], [-0.1, 0.2]]"
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.5
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[[0.1, 0.2], [0.1, -0.2], [-0.1, -0.2], [-0.3, -0.15], [-0.3, 0.15], [-0.1, 0.2]]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.5
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True
map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "map_01.yaml"
map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: False
planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true
behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2
robot_state_publisher:
  ros__parameters:
    use_sim_time: True
waypoint_follower:
  ros__parameters:
    use_sim_time: True
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200
velocity_smoother:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0
```
    
　設定可能なパラメータの一覧は、[Configuration Guide](https://docs.nav2.org/configuration/index.html)からご確認ください(ただし、ここに書かれているものは**humbleではない**ことにご留意ください)。
 　このページには**重要なパラメータのみ**を記します。各フレーム名や`scan_topic`などは、使用するロボットの設定に合わせてください。

##### amcl
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|base_frame_id|baseフレーム名|string|-|“base_footprint”|
|global_frame_id|自己位置推定で基準となるフレーム名|string|-|“map”|
|laser_max_range|最大スキャン距離。<br>-1.0の場合、scanトピックに設定されている最大距離が使用される|double|m|100.0|
|laser_min_range|最小スキャン距離。<br>-1.0の場合、scanトピックに設定されている最小距離が使用される|double|m|-1.0|
|set_initial_pose|ロボットの初期ポーズを設定するか|bool|-|false|
|initial_pose|ロボットの初期ポーズ|geometry_msgs.msg.Pose2D|-|{x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}|
|max_particles|許容されるパーティクルの最大数|int|-|2000|
|min_particles|許容されるパーティクルの最小数|int|-|500|
|odom_frame_id|odomフレーム名|string|-|“odom”|
|robot_model_type|使用するロボットモデル。<br>nav2_amcl::DifferentialMotionModel、nav2_amcl::OmniMotionModel、もしくは独自のプラグインモデルを設定可能|string|-|“nav2_amcl::DifferentialMotionModel”|
|scan_topic|scanトピック名|string|-|“scan”|
|map_topic|mapトピック名|string|-|“map”|

##### bt_navigator
|パラメータ名|説明|型|デフォルト|
|---|---|---|---|
|plugin_lib_names|使用するノードライブラリのリスト|vector<string>|[““]|
|global_frame|環境内の基準となるフレーム名|string|“map”|
|robot_base_frame|baseフレーム名|string|“base_link”|
|odom_topic|odomトピック名|string|“odom”|

##### controller_server
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|controller_frequency|コントローラの実行周期|double|Hz|20.0|
|progress_checker_plugins|ロボットの進行状況を確認するためのチェッカーで使用するプラグインのリスト|vector<string>|-|[“progress_checker”]|
|goal_checker_plugins|目標が達成されたかを確認するためのチェッカーで使用するプラグインのリスト|vector<string>|-|[“goal_checker”]|
|controller_plugins|リクエストとパラメータを処理するコントローラで使用するプラグインのリスト|vector<string>|-|[“FollowPath”]|

###### controller_server.goal_checker (nav2_controller::SimpleProgressChecker)
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|xy_goal_tolerance|目標達成基準を満たす位置の許容誤差|double|m|0.25|
|yaw_goal_tolerance|目標達成基準を満たす回転の許容誤差|double|rad|0.25|
|stateful|ロボットの回転後、再度、位置を評価するか|bool|-|true|

###### controller_server.FollowPath (dwb_core::DWBLocalPlanner)
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|min_vel_x|$x$方向の最小速度|double|$\text{m}/\text{s}$|0.0|
|min_vel_y|$y$方向の最小速度|double|$\text{m}/\text{s}$|0.0|
|max_vel_x|$x$方向の最大速度|double|$\text{m}/\text{s}$|0.0|
|max_vel_y|$y$方向の最大速度|double|$\text{m}/\text{s}$|0.0|
|max_vel_theta|最大角速度|double|$\text{rad}/\text{s}$|0.0|
|min_speed_xy|最小移動速度|double|$\text{m}/\text{s}$|0.0|
|max_speed_xy|最大移動速度|double|$\text{m}/\text{s}$|0.0|
|min_speed_theta|最小角速度|double|$\text{rad}/\text{s}$|0.0|
|acc_lim_x|$x$方向の最大加速度|double|$\text{m}/\text{s}^2$|0.0|
|acc_lim_y|$y$方向の最大加速度|double|$\text{m}/\text{s}^2$|0.0|
|acc_lim_theta|最大角加速度|double|$\text{rad}/\text{s}^2$|0.0|
|decel_lim_x|$x$方向の最大減速度|double|$\text{m}/\text{s}^2$|0.0|
|decel_lim_y|$y$方向の最大減速度|double|$\text{m}/\text{s}^2$|0.0|
|decel_lim_theta|最大角減速度|double|$\text{rad}/\text{s}^2$|0.0|
|critics|使用する評価プラグインのリスト|vector<string>|-|N/A|
|PathAlign.scale|軌道をグローバル経路にどれだけ一致させるかの重み|double|-|32.0|
|GoalAlign.scale|軌道をグローバル目標にどれだけ一致させるかの重み|double|-|24.0|
|PathDist.scale|経路にどれだけ近づこうとするかの重み|double|-|32.0|
|GoalDist.scale|目標にどれだけ近づこうとするかの重み|double|-|24.0|

##### costmap_2d
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|footprint|ロボットが存在している領域を表す座標のセット。文字列として渡す必要がある|string (vector<double>を文字列化したもの)|m|“[]”|
|global_frame|環境内の基準となるフレーム名|string|-|“map”|
|height|コストマップの高さ|int|m|5|
|width|コストマップの幅|int|m|5|
|publish_frequency|コストマップをpublishする頻度|double|Hz|1.0|
|resolution|コストマップの解像度|double|$\text{m}/\text{pixel}$|0.1|
|robot_base_frame|baseフレーム名|string|-|“base_link”|
|robot_radius|ロボットの半径。footprintが指定されていない場合に使用される|double|m|0.1|
|track_unknown_space|この値がfalseの場合、不明な領域は障害物が存在しないものとして扱う。trueの場合、不明な領域のまま扱う|bool|-|false|
|update_frequency|コストマップの更新頻度|double|Hz|5.0|
|plugins|使用するプラグインのリスト|vector<string>|-|[“static_layer”, “obstacle_layer”, “inflation_layer”]|
|filters|使用するフィルタのリスト|vector<string>|-|[““]|

###### costmap_2d.static_layer (nav2_costmap_2d::StaticLayer)
|パラメータ名|説明|型|デフォルト|
|---|---|---|---|
|map_topic|subscribeするmapトピック名|string|“map”|

###### costmap_2d.obstacle_layer (nav2_costmap_2d::ObstacleLayer)
|パラメータ名|説明|型|デフォルト|
|---|---|---|---|
|observation_sources|センサのソース名のリスト。複数使用する場合、スペースで区切る|vector<string>|[““]|

###### costmap_2d.obstacle_layer.&lt;observation_sources&gt; (nav2_costmap_2d::ObstacleLayer.&lt;observation_sources&gt;)
|パラメータ名|説明|型|デフォルト|
|---|---|---|---|
|topic|センサのトピック名|string|“”|
|sensor_frame|センサのフレーム名|string|“”|
|data_type|センサのデータの型。LaserScanまたはPointCloud2|string|“LaserScan”|

###### costmap_2d.inflation_layer (nav2_costmap_2d::InflationLayer)
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|inflation_radius|コストマップにおける障害物を、周囲に膨張させる半径|double|m|0.55|
|cost_scaling_factor|膨張コストの指数関数的減衰係数。障害物からロボットの半径までの領域に膨張コストを設定する。この値を増やすと膨張コストの値が減る|double|-|10.0|
|inflate_unknown|不明な領域を障害物のように扱い、膨張させるか|bool|-|false|
|inflate_around_unknown|不明な領域を膨張させるか|bool|-|false|

##### map_server
|パラメータ名|説明|型|デフォルト|
|---|---|---|---|
|yaml_filename|環境地図のyamlファイルのパス|string|N/A|
|topic_name|読み込まれたmapをpublishするトピック名|string|“map”|
|frame_id|読み込まれたmapをpublishするフレーム名|string|“map”|

##### planner_server
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|planner_plugins|使用するプラグインのリスト|vector<string>|-|[“GridBased”]|
|expected_planner_frequency|計画を算出する頻度の期待値|double|Hz|1.0|

###### planner_server.GridBased (nav2_navfn_planner/NavfnPlanner)
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|tolerance|目標のポーズと経路の最終地点との許容誤差|double|m|0.5|
|allow_unknown|不明な領域を通る経路を許可するか|bool|-|true|

##### behavior_server
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|cycle_frequency|動作プラグインを実行する頻度|double|Hz|10.0|
|robot_base_frame|baseフレーム名|string|-|“base_link”|
|behavior_plugins|使用する動作プラグインのリスト|vector<string>|-|[“spin”, “backup”, “drive_on_heading”, “wait”]|

##### waypoint_follower
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|stop_on_failure|単一のwaypointへの到達が失敗した場合、次のwaypointへの移動を停止するか|bool|-|true|
|loop_rate|現在のNavigationのresultを確認する頻度|int|Hz|20|
|waypoint_task_executor_plugin|各waypointに到達した時に実行されるタスクを定義するプラグイン|string|-|“wait_at_waypoint”|

###### waypoint_follower.wait_at_waypoint (nav2_waypoint_follower::WaitAtWaypoint)
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|waypoint_pause_duration|各waypointに到達した後、ロボットが待機する時間|int|ms|0|

##### velocity_smoother
|パラメータ名|説明|型|単位|デフォルト|
|---|---|---|---|---|
|smoothing_frequency|受信した最新の速度命令を調整する頻度|double|Hz|20.0|
|max_velocity|$x$軸方向、$y$軸方向、回転方向の速度、角速度の最大値|vector<double>|[$\text{m}/\text{s}$ $\text{m}/\text{s}$ $\text{rad}/\text{s}$]|[0.50, 0.0, 2.5]|
|min_velocity|$x$軸方向、$y$軸方向、回転方向の速度、角速度の最小値|vector<double>|[$\text{m}/\text{s}$ $\text{m}/\text{s}$ $\text{rad}/\text{s}$]|[-0.50, 0.0, -2.5]|
|max_accel|$x$軸方向、$y$軸方向、回転方向の加速度、角加速度の最大値|vector<double>|[$\text{m}/\text{s}^2$ $\text{m}/\text{s}^2$ $\text{rad}/\text{s}^2$]|[2.5, 0.0, 3.2]|
|max_decel|$x$軸方向、$y$軸方向、回転方向の加速度、角加速度の最小値|vector<double>|[$\text{m}/\text{s}^2$ $\text{m}/\text{s}^2$ $\text{rad}/\text{s}^2$]|[-2.5, 0.0, -3.2]|
|deadband_velocity|$x$軸方向、$y$軸方向、回転方向の速度、角速度の絶対値の最小値|vector<double>|[$\text{m}/\text{s}$ $\text{m}/\text{s}$ $\text{rad}/\text{s}$]|[0.0, 0.0, 0.0]|

#### rvizファイルの生成
　Navigation2では、Rvizを適切に設定する必要があります。デフォルトで使用される設定ファイルは[nav2_default_view.rviz](https://github.com/ros-navigation/navigation2/blob/humble/nav2_bringup/rviz/nav2_default_view.rviz)です。今回は、デフォルトの設定ファイルをベースに、使用するロボットに合わせて一部変更した、以下の設定ファイルを作成し~/ros2_ws/src/sim_py_01/rvizに配置します(内容の説明については割愛します)。ファイル名はwheel_robot_simple_nav2.rvizとしています。
```~/ros2_ws/src/sim_py_01/rviz/wheel_robot_simple_nav2.rviz
Panels:
  - Class: rviz_common/Displays
    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /TF1/Frames1
        - /TF1/Tree1
      Splitter Ratio: 0.5833333134651184
    Tree Height: 462
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: nav2_rviz_plugins/Navigation 2
    Name: Navigation 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: false
      Description File: ""
      Description Source: Topic
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /robot_description
      Enabled: false
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
      Mass Properties:
        Inertia: false
        Mass: false
      Name: RobotModel
      TF Prefix: ""
      Update Interval: 0
      Value: false
      Visual Enabled: true
    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: false
        back_ball_link:
          Value: true
        base_link:
          Value: true
        body_link:
          Value: true
        front_laser_link:
          Value: true
        left_wheel_link:
          Value: true
        map:
          Value: true
        odom:
          Value: true
        right_wheel_link:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: false
      Tree:
        map:
          odom:
            base_link:
              body_link:
                back_ball_link:
                  {}
                front_laser_link:
                  {}
                left_wheel_link:
                  {}
                right_wheel_link:
                  {}
      Update Interval: 0
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: LaserScan
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /scan
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/Map
      Color Scheme: map
      Draw Behind: true
      Enabled: true
      Name: Map
      Topic:
        Depth: 1
        Durability Policy: Transient Local
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map
      Update Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map_updates
      Use Timestamp: false
      Value: true
    - Alpha: 1
      Arrow Length: 0.019999999552965164
      Axes Length: 0.30000001192092896
      Axes Radius: 0.009999999776482582
      Class: rviz_default_plugins/PoseArray
      Color: 0; 180; 0
      Enabled: true
      Head Length: 0.07000000029802322
      Head Radius: 0.029999999329447746
      Name: Amcl Particle Swarm
      Shaft Length: 0.23000000417232513
      Shaft Radius: 0.009999999776482582
      Shape: Arrow (Flat)
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /particlecloud
      Value: true
    - Class: rviz_common/Group
      Displays:
        - Alpha: 0.30000001192092896
          Class: rviz_default_plugins/Map
          Color Scheme: costmap
          Draw Behind: false
          Enabled: true
          Name: Global Costmap
          Topic:
            Depth: 1
            Durability Policy: Transient Local
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/costmap
          Update Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/costmap_updates
          Use Timestamp: false
          Value: true
        - Alpha: 1
          Buffer Length: 1
          Class: rviz_default_plugins/Path
          Color: 255; 0; 0
          Enabled: true
          Head Diameter: 0.019999999552965164
          Head Length: 0.019999999552965164
          Length: 0.30000001192092896
          Line Style: Lines
          Line Width: 0.029999999329447746
          Name: Path
          Offset:
            X: 0
            Y: 0
            Z: 0
          Pose Color: 255; 85; 255
          Pose Style: Arrows
          Radius: 0.029999999329447746
          Shaft Diameter: 0.004999999888241291
          Shaft Length: 0.019999999552965164
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /plan
          Value: true
        - Alpha: 1
          Class: rviz_default_plugins/Polygon
          Color: 25; 255; 0
          Enabled: true
          Name: Polygon
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /global_costmap/published_footprint
          Value: true
      Enabled: true
      Name: Global Planner
    - Class: rviz_common/Group
      Displays:
        - Alpha: 0.699999988079071
          Class: rviz_default_plugins/Map
          Color Scheme: costmap
          Draw Behind: false
          Enabled: true
          Name: Local Costmap
          Topic:
            Depth: 1
            Durability Policy: Transient Local
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/costmap
          Update Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/costmap_updates
          Use Timestamp: false
          Value: true
        - Alpha: 1
          Buffer Length: 1
          Class: rviz_default_plugins/Path
          Color: 0; 12; 255
          Enabled: true
          Head Diameter: 0.30000001192092896
          Head Length: 0.20000000298023224
          Length: 0.30000001192092896
          Line Style: Lines
          Line Width: 0.029999999329447746
          Name: Local Plan
          Offset:
            X: 0
            Y: 0
            Z: 0
          Pose Color: 255; 85; 255
          Pose Style: None
          Radius: 0.029999999329447746
          Shaft Diameter: 0.10000000149011612
          Shaft Length: 0.10000000149011612
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_plan
          Value: true
        - Class: rviz_default_plugins/MarkerArray
          Enabled: true
          Name: Trajectories
          Namespaces:
            {}
          Topic:
            Depth: 5
            Durability Policy: Volatile
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /marker
          Value: true
        - Alpha: 1
          Class: rviz_default_plugins/Polygon
          Color: 25; 255; 0
          Enabled: true
          Name: Polygon
          Topic:
            Depth: 5
            Durability Policy: Volatile
            Filter size: 10
            History Policy: Keep Last
            Reliability Policy: Reliable
            Value: /local_costmap/published_footprint
          Value: true
      Enabled: true
      Name: Controller
    - Class: rviz_default_plugins/MarkerArray
      Enabled: true
      Name: MarkerArray
      Namespaces:
        {}
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /waypoints
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Covariance x: 0.25
      Covariance y: 0.25
      Covariance yaw: 0.06853891909122467
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
    - Class: nav2_rviz_plugins/GoalTool
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Angle: -1.6150002479553223
      Class: rviz_default_plugins/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Scale: 127.88431549072266
      Target Frame: <Fixed Frame>
      Value: TopDownOrtho (rviz_default_plugins)
      X: -0.044467076659202576
      Y: -0.38726311922073364
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 932
  Hide Left Dock: false
  Hide Right Dock: true
  Navigation 2:
    collapsed: false
  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1545
  X: 375
  Y: 111
```

#### launchファイルの生成
　以下のファイルを~/ros2_ws/src/sim_py_01/launchに配置します。今回、ファイル名はwheel_robot_simple_navigation.launch.pyとしています。
```~/ros2_ws/src/sim_py_01/launch/wheel_robot_simple_navigation.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch.actions import ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    package_dir = get_package_share_directory('sim_py_01')
    urdf = os.path.join(package_dir, 'urdf', 'wheel_robot_simple.urdf')
    rviz = os.path.join(package_dir, 'rviz', 'wheel_robot_simple_nav2.rviz')
    world = os.path.join(package_dir, 'world', 'maze.world')
    map = os.path.join(package_dir, 'maps', 'map_01.yaml')
    nav2_params = os.path.join(package_dir, 'config', 'navigation_params.yaml')
    os.environ['GAZEBO_MODEL_PATH'] = os.path.join(package_dir, 'models')
    nav2_package_dir = get_package_share_directory('nav2_bringup')
    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf],),
        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf],),
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz],),
        ExecuteProcess(
            cmd=['gazebo', '--verbose', '-s',
                 'libgazebo_ros_init.so', '-s',
                 'libgazebo_ros_factory.so', world],
            output='screen',),
        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            name='urdf_spawner',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-topic', '/robot_description',
                       '-entity', 'wheel_robot_simple'],),
        SetRemap(src='cmd_vel', dst='/wheel_robot_simple/cmd_vel'),
        SetRemap(src='odom', dst='/wheel_robot_simple/odom'),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_package_dir, 'launch',
                             'bringup_launch.py')),
            launch_arguments={
                'map': map,
                'use_sim_time': use_sim_time,
                'params_file': nav2_params}.items(),),
    ])
```

#### setup.pyの編集
　新たに作成したmapsディレクトリをビルド対象に含めるため、setup.pyを修正します。以下の記述をdata_files[]の中に追加してください。
```
(os.path.join('share', package_name, 'maps'), glob('maps/*')),
```

#### ビルド
　以下のコマンドを実行してsim_py_01パッケージをビルドします。
```
cd ~/ros2_ws
colcon build --packages-select sim_py_01
source ~/ros2_ws/install/setup.bash
```

#### Navigation2の実行
　以下のコマンドを実行すると、RvizとGazeboが立ち上がります。
```
ros2 launch sim_py_03 wheel_robot_simple_navigation.launch.py
```
|コマンド実行直後のRvizとGazebo|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/17c6ba03-9d0b-1527-67fb-80a77d765c06.png">|

#### 初期ポーズの指定
　Navigation2では、自動走行を行う前に、ロボットの初期ポーズ(initial_pose)を指定する必要があります。Rvizの上部にある2D Pose Estimateをクリックし、地図上でロボットの位置と姿勢を指定します。この時、緑の矢印の根本の部分がロボットの位置を示し、矢印の向きがロボットの姿勢を表します。
|ロボットの初期ポーズの指定|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/b4b8a6cd-f918-5e39-012e-6818935ef6e0.gif">|

#### 自動走行の実行
　自動走行を行う準備が完了しましたので、目的地を設定し、実際に自動走行を行わせていきます。
　Rviz上部にある「Nav2 Goal」をクリックし、先程の2D Pose Estimatesと同様の操作でロボットの目的地と姿勢を指定します。目的地を指定後、自動的に目的地までの経路が引かれ、ロボットが目的地まで自動的に移動します。
|ロボットの自動走行|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/dc435b82-c568-113c-a8fd-1d16fc2dc60a.gif">|

# おわりに
　今回は、ROS 2においてNavigation2を用いた自動走行を行う方法について紹介しました。また、ROS 2 学習用記事としましては、ここで一旦終了となります。ここまでお読みいただいた皆様、誠にありがとうございました。