From 3dd7c324e0d2b7a6f092cd4563e832fe0533ce51 Mon Sep 17 00:00:00 2001
From: chyoo <chyoo@futuredrive.net>
Date: Tue, 22 Sep 2026 11:12:05 +0900
Subject: [PATCH] feat: support Yahboom in shared Nav2 planning

---
 .../config/planning_2d_real_yahboom.yaml      | 66 +++++++++++++++++++
 .../config/planning_2d_sim_yahboom.yaml       | 66 +++++++++++++++++++
 .../launch/nav2_planning.launch.py            |  2 +-
 3 files changed, 133 insertions(+), 1 deletion(-)
 create mode 100644 src/planning_pkg/config/planning_2d_real_yahboom.yaml
 create mode 100644 src/planning_pkg/config/planning_2d_sim_yahboom.yaml

diff --git a/src/planning_pkg/config/planning_2d_real_yahboom.yaml b/src/planning_pkg/config/planning_2d_real_yahboom.yaml
new file mode 100644
index 0000000..e9fd43b
--- /dev/null
+++ b/src/planning_pkg/config/planning_2d_real_yahboom.yaml
@@ -0,0 +1,66 @@
+# Yahboom Pi5 differential-drive profile. Vehicle owns /scan, /odom and TF.
+bt_navigator:
+  ros__parameters:
+    use_sim_time: false
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    default_nav_to_pose_bt_xml: ''
+    default_nav_through_poses_bt_xml: ''
+    navigators: [navigate_to_pose]
+    navigate_to_pose:
+      plugin: nav2_bt_navigator/NavigateToPoseNavigator
+    bt_loop_duration: 20
+    default_server_timeout: 2000
+    wait_for_service_timeout: 4000
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+planner_server:
+  ros__parameters:
+    use_sim_time: false
+    expected_planner_frequency: 20.0
+    planner_plugins: [GridBased]
+    GridBased:
+      plugin: nav2_navfn_planner/NavfnPlanner
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: false
+      global_frame: map
+      robot_base_frame: base_footprint
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      resolution: 0.05
+      footprint: "[[-0.165,-0.08],[-0.165,0.08],[0.125,0.08],[0.125,-0.08]]"
+      track_unknown_space: true
+      plugins: [static_layer, obstacle_layer, inflation_layer]
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        map_subscribe_transient_local: true
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        observation_sources: scan
+        scan:
+          topic: /scan
+          data_type: LaserScan
+          marking: true
+          clearing: true
+          max_obstacle_height: 1.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        inflation_radius: 0.30
+        cost_scaling_factor: 3.0
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: false
+    loop_rate: 20
+    stop_on_failure: false
diff --git a/src/planning_pkg/config/planning_2d_sim_yahboom.yaml b/src/planning_pkg/config/planning_2d_sim_yahboom.yaml
new file mode 100644
index 0000000..e5d542c
--- /dev/null
+++ b/src/planning_pkg/config/planning_2d_sim_yahboom.yaml
@@ -0,0 +1,66 @@
+# Yahboom Pi5 differential-drive profile. Vehicle owns /scan, /odom and TF.
+bt_navigator:
+  ros__parameters:
+    use_sim_time: true
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    default_nav_to_pose_bt_xml: ''
+    default_nav_through_poses_bt_xml: ''
+    navigators: [navigate_to_pose]
+    navigate_to_pose:
+      plugin: nav2_bt_navigator/NavigateToPoseNavigator
+    bt_loop_duration: 20
+    default_server_timeout: 2000
+    wait_for_service_timeout: 4000
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: true
+
+planner_server:
+  ros__parameters:
+    use_sim_time: true
+    expected_planner_frequency: 20.0
+    planner_plugins: [GridBased]
+    GridBased:
+      plugin: nav2_navfn_planner/NavfnPlanner
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: true
+      global_frame: map
+      robot_base_frame: base_footprint
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      resolution: 0.05
+      footprint: "[[-0.165,-0.08],[-0.165,0.08],[0.125,0.08],[0.125,-0.08]]"
+      track_unknown_space: true
+      plugins: [static_layer, obstacle_layer, inflation_layer]
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        map_subscribe_transient_local: true
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        observation_sources: scan
+        scan:
+          topic: /scan
+          data_type: LaserScan
+          marking: true
+          clearing: true
+          max_obstacle_height: 1.0
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        inflation_radius: 0.30
+        cost_scaling_factor: 3.0
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: true
+    loop_rate: 20
+    stop_on_failure: false
diff --git a/src/planning_pkg/launch/nav2_planning.launch.py b/src/planning_pkg/launch/nav2_planning.launch.py
index 974cba6..3ebbe6d 100755
--- a/src/planning_pkg/launch/nav2_planning.launch.py
+++ b/src/planning_pkg/launch/nav2_planning.launch.py
@@ -15,7 +15,7 @@ from nav2_common.launch import RewrittenYaml
 ARGUMENTS = [
     DeclareLaunchArgument('sim_mode', default_value='true'),
     DeclareLaunchArgument('log_level', default_value='info'),
-    DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'turtlebot3']),
+    DeclareLaunchArgument('robot', default_value='fr09', choices=['mkmini', 'fr09', 'turtlebot3', 'yahboom']),
     DeclareLaunchArgument('level', default_value='3d'),
 ]