feat: support Yahboom in shared Nav2 control

src/control_pkg/config/control_2d_real_yahboom.yaml

@@ -0,0 +1,112 @@
+# Yahboom Pi5 differential-drive profile. Vehicle owns /scan, /odom and TF.
+controller_server:
+  ros__parameters:
+    use_sim_time: false
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.0
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.5
+    progress_checker_plugin: progress_checker
+    goal_checker_plugins: [goal_checker]
+    controller_plugins: [FollowPath]
+    progress_checker:
+      plugin: nav2_controller::SimpleProgressChecker
+      required_movement_radius: 0.08
+      movement_time_allowance: 10.0
+    goal_checker:
+      plugin: nav2_controller::SimpleGoalChecker
+      xy_goal_tolerance: 0.15
+      yaw_goal_tolerance: 0.2
+      stateful: true
+    FollowPath:
+      plugin: dwb_core::DWBLocalPlanner
+      debug_trajectory_details: false
+      min_vel_x: -0.05
+      max_vel_x: 0.2
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.2
+      acc_lim_x: 1.5
+      decel_lim_x: -1.5
+      acc_lim_theta: 3.2
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vtheta_samples: 20
+      sim_time: 1.7
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.15
+      trans_stopped_velocity: 0.05
+      short_circuit_trajectory_evaluation: true
+      stateful: true
+      critics: [RotateToGoal, Oscillation, BaseObstacle, GoalAlign, PathAlign, PathDist, GoalDist]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 24.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 18.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 24.0
+      GoalDist.scale: 18.0
+      RotateToGoal.scale: 24.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: false
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[-0.165,-0.08],[-0.165,0.08],[0.125,0.08],[0.125,-0.08]]"
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      plugins: [obstacle_layer, inflation_layer]
+      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
+
+behavior_server:
+  ros__parameters:
+    use_sim_time: false
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: [backup, drive_on_heading, wait]
+    backup:
+      plugin: nav2_behaviors/BackUp
+    drive_on_heading:
+      plugin: nav2_behaviors/DriveOnHeading
+    wait:
+      plugin: nav2_behaviors/Wait
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.2
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: false
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: OPEN_LOOP
+    max_velocity: [0.2, 0.0, 1.0]
+    min_velocity: [-0.05, 0.0, -1.0]
+    max_accel: [1.5, 0.0, 3.2]
+    max_decel: [-1.5, 0.0, -3.2]
+    odom_topic: /odom
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

src/control_pkg/config/control_2d_sim_mkmini.yaml

@@ -4,7 +4,9 @@
 controller_server:
   ros__parameters:
     use_sim_time: True
-    controller_frequency: 10.0
+    # Reeds-Shepp paths switch direction at cusps. A faster control loop keeps
+    # the simulated tricycle from overshooting those short reverse segments.
+    controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.0
     min_theta_velocity_threshold: 0.001
@@ -24,9 +26,8 @@ controller_server:
       stateful: True
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      # Reverse remains available to Nav2 recovery behaviours, but the normal
+      # Required to track reverse segments generated by the Reeds-Shepp planner.
-      # path follower must not alternate direction along a goal path.
+      allow_reversing: true
-      allow_reversing: false
       use_dynamic_window: false
       use_velocity_scaled_lookahead_dist: false
       use_rotate_to_heading: false
@@ -34,16 +35,16 @@ controller_server:
       use_regulated_linear_velocity_scaling: true
       use_cost_regulated_linear_velocity_scaling: true
 
-      # A 1 m lookahead at 0.5 m/s cuts across the 1.35 m-radius vehicle
+      # Keep the carrot before the next direction-change cusp. A long
-      # paths. Keep the simulated tricycle close to the planned arc.
+      # lookahead makes a car-like controller cut across short reverse arcs.
-      desired_linear_vel: 0.3
+      desired_linear_vel: 0.4
-      min_approach_linear_velocity: 0.15
+      min_approach_linear_velocity: 0.08
-      lookahead_dist: 0.6
+      lookahead_dist: 0.35
       cost_scaling_dist: 0.6
       cost_scaling_gain: 1.0
       approach_velocity_scaling_dist: 1.0
       regulated_linear_scaling_min_radius: 1.35
-      regulated_linear_scaling_min_speed: 0.3
+      regulated_linear_scaling_min_speed: 0.1
       max_allowed_time_to_collision_up_to_carrot: 2.0
 
 local_costmap:
@@ -112,7 +113,7 @@ behavior_server:
 velocity_smoother:
   ros__parameters:
     use_sim_time: True
-    smoothing_frequency: 10.0
+    smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
     max_velocity: [0.5, 0.0, 0.5]

src/control_pkg/config/control_2d_sim_yahboom.yaml

@@ -0,0 +1,112 @@
+# Yahboom Pi5 differential-drive profile. Vehicle owns /scan, /odom and TF.
+controller_server:
+  ros__parameters:
+    use_sim_time: true
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.0
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.5
+    progress_checker_plugin: progress_checker
+    goal_checker_plugins: [goal_checker]
+    controller_plugins: [FollowPath]
+    progress_checker:
+      plugin: nav2_controller::SimpleProgressChecker
+      required_movement_radius: 0.08
+      movement_time_allowance: 10.0
+    goal_checker:
+      plugin: nav2_controller::SimpleGoalChecker
+      xy_goal_tolerance: 0.15
+      yaw_goal_tolerance: 0.2
+      stateful: true
+    FollowPath:
+      plugin: dwb_core::DWBLocalPlanner
+      debug_trajectory_details: false
+      min_vel_x: -0.05
+      max_vel_x: 0.2
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.2
+      acc_lim_x: 1.5
+      decel_lim_x: -1.5
+      acc_lim_theta: 3.2
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vtheta_samples: 20
+      sim_time: 1.7
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.15
+      trans_stopped_velocity: 0.05
+      short_circuit_trajectory_evaluation: true
+      stateful: true
+      critics: [RotateToGoal, Oscillation, BaseObstacle, GoalAlign, PathAlign, PathDist, GoalDist]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 24.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 18.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 24.0
+      GoalDist.scale: 18.0
+      RotateToGoal.scale: 24.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: true
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[-0.165,-0.08],[-0.165,0.08],[0.125,0.08],[0.125,-0.08]]"
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      plugins: [obstacle_layer, inflation_layer]
+      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
+
+behavior_server:
+  ros__parameters:
+    use_sim_time: true
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: [backup, drive_on_heading, wait]
+    backup:
+      plugin: nav2_behaviors/BackUp
+    drive_on_heading:
+      plugin: nav2_behaviors/DriveOnHeading
+    wait:
+      plugin: nav2_behaviors/Wait
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.2
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: true
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: OPEN_LOOP
+    max_velocity: [0.2, 0.0, 1.0]
+    min_velocity: [-0.05, 0.0, -1.0]
+    max_accel: [1.5, 0.0, 3.2]
+    max_decel: [-1.5, 0.0, -3.2]
+    odom_topic: /odom
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

src/control_pkg/launch/nav2_control.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='turtlebot3', choices=['mkmini', 'fr09', 'turtlebot3']),
+    DeclareLaunchArgument('robot', default_value='turtlebot3', choices=['mkmini', 'fr09', 'turtlebot3', 'yahboom']),
     DeclareLaunchArgument('level', default_value='2d'),
 ]