feat: support Yahboom in shared Nav2 planning

src/planning_pkg/behavior/navigate_to_pose_no_spin.xml

@@ -5,7 +5,10 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <PipelineSequence name="NavigateToPose">
-      <RateController hz="1.0">
+      <!-- Reeds-Shepp direction choices can flip when a car-like path is
+           recomputed every second. The static Gazebo world only needs a
+           low-rate refresh while the controller follows the current arc. -->
+      <RateController hz="0.1">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
       </RateController>
       <FollowPath path="{path}" controller_id="FollowPath"/>

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

src/planning_pkg/config/planning_2d_sim_mkmini.yaml

@@ -81,24 +81,10 @@ global_costmap:
       footprint: "[ [0.44, 0.42], [0.44, -0.42], [-0.44, -0.42], [-0.44, 0.42] ]"
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "inflation_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
-      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"
-          raytrace_max_range: 8.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 6.0
-          obstacle_min_range: 0.0
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 2.0
@@ -121,16 +107,16 @@ planner_server:
       max_iterations: 3000000
       max_on_approach_iterations: 5000
       max_planning_time: 10.0
-      # Normal navigation should remain forward-only. Reeds-Shepp creates
+      # Test car-like paths that can reverse to leave constrained spaces.
-      # forward/reverse cusps that Regulated Pure Pursuit can oscillate on.
+      motion_model_for_search: "REEDS_SHEPP"
-      motion_model_for_search: "DUBIN"
       angle_quantization_bins: 144
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 5.0
       # tricycle_drive uses a 0.72 m wheelbase with a 0.5 rad steering limit.
       # 0.72 / tan(0.5) is 1.32 m; keep a small planning margin.
       minimum_turning_radius: 1.35
-      change_penalty: 0.0
+      # Prefer a continuous arc over avoidable forward/reverse oscillation.
+      change_penalty: 1.0
       non_straight_penalty: 1.5
       cost_penalty: 5.0
       retrospective_penalty: 0.1

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

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'),
 ]