はじめに
今回も引き続き、ROS 2において、対向二輪型ロボットに自動走行を行わせる方法について記載します。今回の記事では、前回作成した環境地図と、Navigation2を用いて、ロボットに自動走行を行わせていきます。
使用するロボットや環境、地図については、前回のものを流用しています。
前提条件
今回の記事は以下の環境で動かすこと前提に記載しています。
| 条件 | |
|---|---|
| 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 |
|---|
 |
| 参照: Detailed Behavior Tree Walkthrough — Nav2 1.0.0 documentation |
BT内の具体的な行動を実行するために、Navigation2では4つのアクションサーバー、Planner、Controller、Behavior、Smootherが用意されています。Plannerは目的地までの経路を計算し、Controllerはその経路をロボットが辿るための制御を行います。Behaviorはロボットが動けなくなった場合などの障害が発生したときに回復動作を行うために使用され、Smootherは必要に応じてPlannerによって求められた経路を最適化します。
| Navigation2の構造 |
|---|
 |
| 参照: Nav2 — Nav2 1.0.0 documentation |
また、環境を認識するためにコストマップ(Cost map)という表現が使われます。コストマップは、不明(unknown)、空き(free)、占有(occupied)、膨張コスト(inflated cost)を含む領域で構成された2次元地図です。このコストマップは、経路を計算したり、ロボットの制御を行ったりするために使用されます。コストマップは複数の層で構成されており、環境地図から生成される層(static_layer)やLiDARなどのセンサから生成される層(obstacle_layer)の他、独自の層を追加することも可能です。
さらに、フィルタを適用することで、マップの特定の位置で特定のアクションを発生させることができ、例えば、ロボットが侵入できない立ち入り禁止ゾーンや速度制限エリアなどを設定することができます。
| Navigation2のデフォルトのコストマップ |
|---|
 |
SLAMと同様、Navigation2での自己位置推定にはオドメトリなどが使用されますが、Navigation2ではオドメトリの誤差を吸収するために、Adaptive Monte Carlo Localization(AMCL)などの手法が使用されます。AMCLでは、パーティクルフィルタ(Particle Filter)という手法を用いて、環境地図を基にロボットの現在位置を確率的に推定します。まず、パーティクル(Particle)と呼ばれる仮想的な点を地図上に複数生成します。これらのパーティクルは、ロボットの現在位置の可能性がある場所を示しています。次に、各パーティクルは、LiDARなどのセンサから得られる情報と地図情報を比較し、どれだけ正確に位置を表しているか(尤度)に基づいて重みが付けられます。この重み付けの結果に基づいて、より正確な位置にパーティクルを移動させていくことで、位置推定の精度を向上させていきます。
| AMCL |
|---|
 |
Navigation2の実装
ここからは、前回作成した地図を基に、実際にNavigation2を使用して、ロボットに自動走行を行わせていきます。
Navigation2関連パッケージのインストール
まずNavigation2関連パッケージのインストールを行います。以下のコマンドを実行してください。
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
Navigation2の設定ファイルの用意
Navigation2の設定ファイルを用意します。デフォルトの設定ファイルは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からご確認ください(ただし、ここに書かれているものはhumbleではないことにご留意ください)。
このページには重要なパラメータのみを記します。各フレーム名やscan_topicなどは、使用するロボットの設定に合わせてください。
amcl
| パラメータ名 | 説明 | 型 | 単位 | デフォルト |
|---|---|---|---|---|
| base_frame_id | baseフレーム名 | string | - | “base_footprint” |
| global_frame_id | 自己位置推定で基準となるフレーム名 | string | - | “map” |
| laser_max_range | 最大スキャン距離。 -1.0の場合、scanトピックに設定されている最大距離が使用される |
double | m | 100.0 |
| laser_min_range | 最小スキャン距離。 -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 | 使用するロボットモデル。 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 | [““] |
| 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 | - | [“progress_checker”] |
| goal_checker_plugins | 目標が達成されたかを確認するためのチェッカーで使用するプラグインのリスト | vector | - | [“goal_checker”] |
| controller_plugins | リクエストとパラメータを処理するコントローラで使用するプラグインのリスト | vector | - | [“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 | - | 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を文字列化したもの) | 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 | - | [“static_layer”, “obstacle_layer”, “inflation_layer”] |
| filters | 使用するフィルタのリスト | vector | - | [““] |
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 | [““] |
costmap_2d.obstacle_layer.<observation_sources> (nav2_costmap_2d::ObstacleLayer.<observation_sources>)
| パラメータ名 | 説明 | 型 | デフォルト |
|---|---|---|---|
| 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 | - | [“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 | - | [“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 | [$\text{m}/\text{s}$ $\text{m}/\text{s}$ $\text{rad}/\text{s}$] | [0.50, 0.0, 2.5] |
| min_velocity | $x$軸方向、$y$軸方向、回転方向の速度、角速度の最小値 | vector | [$\text{m}/\text{s}$ $\text{m}/\text{s}$ $\text{rad}/\text{s}$] | [-0.50, 0.0, -2.5] |
| max_accel | $x$軸方向、$y$軸方向、回転方向の加速度、角加速度の最大値 | vector | [$\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 | [$\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 | [$\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です。今回は、デフォルトの設定ファイルをベースに、使用するロボットに合わせて一部変更した、以下の設定ファイルを作成し~/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 |
|---|
 |
初期ポーズの指定
Navigation2では、自動走行を行う前に、ロボットの初期ポーズ(initial_pose)を指定する必要があります。Rvizの上部にある2D Pose Estimateをクリックし、地図上でロボットの位置と姿勢を指定します。この時、緑の矢印の根本の部分がロボットの位置を示し、矢印の向きがロボットの姿勢を表します。
| ロボットの初期ポーズの指定 |
|---|
 |
自動走行の実行
自動走行を行う準備が完了しましたので、目的地を設定し、実際に自動走行を行わせていきます。
Rviz上部にある「Nav2 Goal」をクリックし、先程の2D Pose Estimatesと同様の操作でロボットの目的地と姿勢を指定します。目的地を指定後、自動的に目的地までの経路が引かれ、ロボットが目的地まで自動的に移動します。
| ロボットの自動走行 |
|---|
 |
おわりに
今回は、ROS 2においてNavigation2を用いた自動走行を行う方法について紹介しました。また、ROS 2 学習用記事としましては、ここで一旦終了となります。ここまでお読みいただいた皆様、誠にありがとうございました。