reduce feasibilty check lookahead poses, finetune params for narrow path, clear local costmap before narrow driving (#20)

Author: jplapp

nav2_bringup/bringup/params/forklift_params.yaml

@@ -20,16 +20,16 @@ controller_server:
         type: "line"
         line_end: [1.9, 0.0]
         line_start: [0.1, 0.0]
-      min_obstacle_dist: 0.5
+      min_obstacle_dist: 0.45
       include_dynamic_obstacles:  False
       costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
       costmap_converter_spin_thread: True
       costmap_converter_rate: 2
       costmap_converter/CostmapToPolygonsDBSMCCH:
-        cluster_max_distance: 0.4
+        cluster_max_distance: 0.2
         cluster_min_pts: 3
         cluster_max_pts: 10
-        convex_hull_min_pt_separation: 0.2
+        convex_hull_min_pt_separation: 0.05
       include_costmap_obstacles: True
       enable_homotopy_class_planning: True
       enable_multithreading: True
@@ -40,9 +40,9 @@ controller_server:
       dt_hysteresis: 0.1
       global_plan_overwrite_orientation: False
       # was 25
-      feasibility_check_no_poses: 20
+      feasibility_check_no_poses: 4
       optimization_verbose: False
-      max_global_plan_lookahead_dist: 4.0
+      max_global_plan_lookahead_dist: 0.7
       max_vel_x: 0.8
       max_vel_x_near_goal: 0.2
       max_vel_x_backwards: 0.3
@@ -66,9 +66,11 @@ controller_server:
       obstacle_proximity_upper_bound: 1.0
       costmap_obstacles_behind_robot_dist: 1.0
       weight_obstacle: 50.0
-      obstacle_poses_affected: 30
+      obstacle_poses_affected: 50
       weight_kinematics_forward_drive: 0.3
       obstacle_association_force_inclusion_factor: 1.0
+      divergence_detection: False
+
 
     GeneralFollowPath:
       plugin: "teb_local_planner::TebLocalPlannerROS"
@@ -90,7 +92,7 @@ controller_server:
       dt_hysteresis: 0.1
       global_plan_overwrite_orientation: False
       # was 25
-      feasibility_check_no_poses: 20
+      feasibility_check_no_poses: 4
       optimization_verbose: False
       max_global_plan_lookahead_dist: 4.0
       max_vel_x: 0.8
@@ -119,6 +121,7 @@ controller_server:
       obstacle_poses_affected: 20
       weight_kinematics_forward_drive: 0.3
       obstacle_association_force_inclusion_factor: 1.0
+      divergence_detection: False
 
     PalletPickupFollowPath:
       plugin: "teb_local_planner::TebLocalPlannerROS"
@@ -141,7 +144,7 @@ controller_server:
       dt_hysteresis: 0.1
       global_plan_overwrite_orientation: False
       # was 25
-      feasibility_check_no_poses: 20
+      feasibility_check_no_poses: 4
       optimization_verbose: False
       max_global_plan_lookahead_dist: 4.0
       max_vel_x: 0.8
@@ -170,6 +173,7 @@ controller_server:
       obstacle_poses_affected: 20
       weight_kinematics_forward_drive: 0.3
       obstacle_association_force_inclusion_factor: 1.0
+      divergence_detection: False
 
     LaneFollowPath:
       plugin: "teb_local_planner::TebLocalPlannerROS"
@@ -192,7 +196,7 @@ controller_server:
       dt_hysteresis: 0.1
       global_plan_overwrite_orientation: False
       # was 25
-      feasibility_check_no_poses: 20
+      feasibility_check_no_poses: 4
       optimization_verbose: False
       max_global_plan_lookahead_dist: 4.0
       max_vel_x: 0.8
@@ -221,6 +225,7 @@ controller_server:
       obstacle_poses_affected: 20
       weight_kinematics_forward_drive: 0.3
       obstacle_association_force_inclusion_factor: 1.0
+      divergence_detection: False
 
 controller_server_rclcpp_node:
   ros__parameters:

nav2_bt_navigator/behavior_trees/drive_between_pallets.xml

@@ -4,14 +4,18 @@
 
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <PipelineSequence name="DriveBetweenPallets">
-      <RateController hz="1.0">
-        <Sequence name="AdjustAndReplan">
-          <AdjustPalletGoal input_goal="{goal}" goal="{adjusted_goal}"/>
-          <ComputePathToPosePython goal="{adjusted_goal}" path="{path}" planner_id="GridBased"/>
-        </Sequence>          
-      </RateController>
-      <FollowPath path="{path}" controller_id="NarrowFollowPath"/>
-    </PipelineSequence>
+    <Sequence name="planning">
+      <Wait wait_duration="1"/>
+      <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+      <PipelineSequence name="DriveBetweenPallets">
+        <RateController hz="1.0">
+          <Sequence name="AdjustAndReplan">
+            <AdjustPalletGoal input_goal="{goal}" goal="{adjusted_goal}"/>
+            <ComputePathToPosePython goal="{adjusted_goal}" path="{path}" planner_id="GridBased"/>
+          </Sequence>
+        </RateController>
+        <FollowPath path="{path}" controller_id="NarrowFollowPath"/>
+      </PipelineSequence>
+    </Sequence>
   </BehaviorTree>
 </root>