navigation2のSmoother Serverを使う

Last updated at 2025-12-28Posted at 2025-12-28

1. はじめに

ROS 2のnavigation2にはSmoother Serverという機能があります。この機能の使い方はWeb上にも公式チュートリアル以外にほとんど情報がありません。これまで使ったことがなかったのでこれを機会に使ってみることにしました。

2. 動作確認環境

以下の環境で動作確認しました。

Ubuntu 24.04
ROS 2 Jazzy

3. Smoother Serverとは

端的に言うと、Smoother Serverは、Planner Serverで生成される経路をスムージングする処理部です。Smoother Serverがない場合、Planner Serverで生成されるpathをController Serverで経路追従します。

Smoother Serverがある場合、Planner Serverで生成されるpathをSmoother Serverがスムージングし、スムージング後のsmooth_pathをController Serverで経路追従します。

4. Smoother Serverを使ってみる

基本的にはhttps://docs.nav2.org/tutorials/docs/adding_smoother.htmlを読むとよいです。ただし、ROS 2ディストリビューションによってInput Ports、Output Portsの仕様が異なることがあるため、お使いのROS 2ディストリビューションに応じてドキュメントの記述を読み替える必要があります。例えば、ROS 2 Jazzyで使う場合、以下の点に注意が必要です。

公式ドキュメントでは

<SmoothPath unsmoothed_path="{path}" smoothed_path="{path}" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>

<WouldASmootherRecoveryHelp error_code="{smoother_error_code}" error_msg="{smoother_error_msg}">

とありますが、ROS 2 JazzyのSmoothPathにerror_msgがないため注意が必要です。以降、kachaka_ros2_dev_kitをベースにしてSmoother Serverを使ってみることとします。

4.1 smoother_pluginsを記述する

Smoother Serverではスムージングアルゴリズムを簡単に切り替えられるようプラグイン形式となっています。今回、smoother_pluginsとして以下のプラグインを指定します。

そのため、kachaka_nav2_bringup/params/nav2_params.yamlでsmoother_serverのパラメータ設定を以下のようにします。

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["simple_smoother", "constrained_smoother", "savitzky_golay_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True
    constrained_smoother:
      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
      reversing_enabled: true
      path_downsampling_factor: 3
      path_upsampling_factor: 1 
      keep_start_orientation: true
      keep_goal_orientation: true
      minimum_turning_radius: 0.40
      w_curve: 30.0
      w_dist: 0.0
      w_smooth: 2000000.0
      w_cost: 0.015
      w_cost_cusp_multiplier: 3.0
      cusp_zone_length: 2.5
      optimizer:
        max_iterations: 70
        debug_optimizer: false
        gradient_tol: 5e3
        fn_tol: 1.0e-15
        param_tol: 1.0e-20
    savitzky_golay_smoother:
      plugin: "nav2_smoother::SavitzkyGolaySmoother"
      window_size: 7
      poly_order: 3
      do_refinement: True
      refinement_num: 2
      enforce_path_inversion: True

4.2 BT XMLファイルを作る

次にSmoother Serverを使うBT XMLファイルを作ります。今回、nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlをベースにして作っていきます。

後からアルゴリズムを切り替えできるようにSmootherSelectorを追加し、デフォルトのアルゴリズムをsimple_smootherにします。選択されたsmootherはselected_smootherに代入されます。

<SmootherSelector selected_smoother="{selected_smoother}" default_smoother="simple_smoother" topic_name="smoother_selector"/>

次にpath（ComputePathToPoseが生成するパス）を入力としてsmooth_path（スムージング後のパス）を生成するようなSmoothPathを追加します。また、smoother_idとしてselected_smootherを指定します。

<SmoothPath unsmoothed_path="{path}" smoothed_path="{smooth_path}" smoother_id="{selected_smoother}" error_code_id="{smoother_error_code}"/>

その後、FollowPathで追従する対象のパスをsmooth_path（スムージング後のパス）に変更します。

<FollowPath path="{smooth_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>

これらの変更を加えたBT XMLファイルの例は以下の通りです。今回、/home/jazzy/navigate_to_pose_w_replanning_and_recovery.xmlとして保存します。

<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <SmootherSelector selected_smoother="{selected_smoother}" default_smoother="simple_smoother" topic_name="smoother_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <RateController hz="1.0">
          <Sequence>
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
            <SmoothPath unsmoothed_path="{path}" smoothed_path="{smooth_path}" smoother_id="{selected_smoother}" error_code_id="{smoother_error_code}"/>
          </Sequence>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{smooth_path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>
      </PipelineSequence>
      <Sequence>
        <Fallback>
          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
          <WouldASmootherRecoveryHelp error_code="{smoother_error_code}"/>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
        </Fallback>
        <ReactiveFallback name="RecoveryFallback">
          <GoalUpdated/>
          <RoundRobin name="RecoveryActions">
            <Sequence name="ClearingActions">
              <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
            <Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_code_id}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

4.3 BT XMLファイルを参照する設定にする

bt_navigatorパッケージのパラメータを記述したパラメータファイル（kachaka_nav2_bringup/params/nav2_params.yaml）で以下の記述をします。

まず、default_nav_to_pose_bt_xmlで先ほど作成したBT XMLファイルを参照するようにします。必要に応じてdefault_nav_through_poses_bt_xmlも設定してください。

    default_nav_to_pose_bt_xml: "/home/jazzy/navigate_to_pose_w_replanning_and_recovery.xml"

また、エラーメッセージをハンドリングできるようにerror_code_namesにsmoother_error_codeを追加します。

    error_code_names:
      - compute_path_error_code
      - smoother_error_code
      - follow_path_error_code

4.4 動作確認

緑のパスがスムージング前のパス、赤いパスがスムージング後のパスです。

また、RViz2のSelectorパネルでSmootherのアルゴリズムを切り替えることができます。

5. その他

5.1 パスを上書きできるのか？

https://docs.nav2.org/tutorials/docs/adding_smoother.htmlに以下の記述があります。

<Sequence name="ComputeAndSmoothPath">
  <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
  <SmoothPath unsmoothed_path="{path}" smoothed_path="{path}" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>
</Sequence>

SmoothPathは、ComputePathToPoseで得られるpathを入力とし、同名のpathを出力するような設定になっています。しかし、このような指定をすると以下のようなエラーが出てしまいました。

[component_container_isolated-10] [WARN] [1766655650.570704030] [controller_server]: [follow_path] [ActionServer] Aborting handle.

https://github.com/ros-navigation/navigation2/issues/5817でディスカッション中です。

5.2 nav2_bringupのnavigation_launch.pyを使うと自動的にSmoother Serverが有効になっている？

nav2_bringupのlaunchファイルを見るとsmoother_serverの起動、パラメータ記述があり、一見するとデフォルトでSmoother Serverが動いていそうに思えます。

Node(
    package='nav2_smoother',
    executable='smoother_server',
    name='smoother_server',
    output='screen',
    respawn=use_respawn,
    respawn_delay=2.0,
    parameters=[configured_params],
    arguments=['--ros-args', '--log-level', log_level],
    remappings=remappings,
),

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

しかし、nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlを読むとSmoothPathがなくそのままだとSmoother Serverが有効化されていないことがわかります。

6. おわりに

今回、navigation2にはSmoother Serverという機能を使ってみました。この記事を読んで興味持った方が居れば使ってみるのはどうでしょうか？

参考URL

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up