improving collision behavior in MPPI (#4009)

* improving collision behavior in MPPI * update readme * fix test updates * adjust nav through poses for contextual error codes * contextual error codes to Nav Through Poses
ros-navigation · Dec 13, 2023 · 04c6ca5 · 04c6ca5
1 parent 1135649
commit 04c6ca5
Show file tree

Hide file tree

Showing 8 changed files with 57 additions and 30 deletions.
diff --git a/.../include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp b/.../include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp
@@ -18,6 +18,7 @@
 #include <string>
 
 #include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav2_msgs/action/compute_path_through_poses.hpp"
 #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
 
 namespace nav2_behavior_tree
@@ -27,6 +28,8 @@ class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
 {
   using Action = nav2_msgs::action::ComputePathToPose;
   using ActionResult = Action::Result;
+  using ThroughAction = nav2_msgs::action::ComputePathThroughPoses;
+  using ThroughActionResult = Action::Result;
 
 public:
   WouldAPlannerRecoveryHelp(

diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp
@@ -26,7 +26,10 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp(
   error_codes_to_check_ = {
     ActionResult::UNKNOWN,
     ActionResult::NO_VALID_PATH,
-    ActionResult::TIMEOUT
+    ActionResult::TIMEOUT,
+    ThroughActionResult::UNKNOWN,
+    ThroughActionResult::TIMEOUT,
+    ThroughActionResult::NO_VALID_PATH
   };
 }
 

diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
@@ -199,7 +199,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.70
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -256,7 +256,7 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.7
       always_send_full_costmap: True
 
 # The yaml_filename does not need to be specified since it going to be set by defaults in launch.
@@ -362,7 +362,7 @@ collision_monitor:
       type: "polygon"
       action_type: "approach"
       footprint_topic: "/local_costmap/published_footprint"
-      time_before_collision: 2.0
+      time_before_collision: 1.2
       simulation_time_step: 0.1
       min_points: 6
       visualize: False

diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
@@ -13,26 +13,38 @@
               <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
               <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
             </ReactiveSequence>
-            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            <Sequence>
+              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            </Sequence>
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
-          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          <Sequence>
+            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          </Sequence>
         </RecoveryNode>
       </PipelineSequence>
-      <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.05" error_code_id="{backup_error_code}"/>
-        </RoundRobin>
-      </ReactiveFallback>
+      <Sequence>
+        <Fallback>
+          <WouldAControllerRecoveryHelp error_code="{follow_path_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.05" error_code_id="{backup_error_code}"/>
+          </RoundRobin>
+        </ReactiveFallback>
+      </Sequence>
     </RecoveryNode>
   </BehaviorTree>
 </root>
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -104,7 +104,7 @@ This process is then repeated a number of times and returns a converged solution
  | critical_weight          | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions.                                                                |
  | repulsion_weight          | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles.                                                                |
  | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
- | collision_cost       | double | Default 10000.0. Cost to apply to a true collision in a trajectory.                                          |
+ | collision_cost       | double | Default 100000.0. Cost to apply to a true collision in a trajectory.                                          |
  | collision_margin_distance   | double    | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. |
  | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.   
 
@@ -124,11 +124,11 @@ Note: There is a "Legacy" version of this critic also available with the same pa
 #### Path Angle Critic
  | Parameter                 | Type   | Definition                                                                                                  |
  | ---------------           | ------ | ----------------------------------------------------------------------------------------------------------- |
- | cost_weight               | double | Default 2.0. Weight to apply to critic term.                                                                |
+ | cost_weight               | double | Default 2.2. Weight to apply to critic term.                                                                |
  | cost_power                | int    | Default 1. Power order to apply to term.                                                                    |
  | threshold_to_consider     | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered             |
  | offset_from_furthest      | int    | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to.  |
- | max_angle_to_furthest     | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered           |
+ | max_angle_to_furthest     | double | Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered           |
  | mode     | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. |
 
 

diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -25,7 +26,7 @@ void ObstaclesCritic::initialize()
   getParam(power_, "cost_power", 1);
   getParam(repulsion_weight_, "repulsion_weight", 1.5);
   getParam(critical_weight_, "critical_weight", 20.0);
-  getParam(collision_cost_, "collision_cost", 10000.0);
+  getParam(collision_cost_, "collision_cost", 100000.0);
   getParam(collision_margin_distance_, "collision_margin_distance", 0.10);
   getParam(near_goal_distance_, "near_goal_distance", 0.5);
 
@@ -160,18 +161,26 @@ void ObstaclesCritic::score(CriticData & data)
       // Let near-collision trajectory points be punished severely
       if (dist_to_obj < collision_margin_distance_) {
         traj_cost += (collision_margin_distance_ - dist_to_obj);
-      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
-        repulsive_cost[i] += (inflation_radius_ - dist_to_obj);
+      }
+
+      // Generally prefer trajectories further from obstacles
+      if (!near_goal) {
+        repulsive_cost[i] += inflation_radius_ - dist_to_obj;
       }
     }
 
     if (!trajectory_collide) {all_trajectories_collide = false;}
     raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
   }
 
+  // Normalize repulsive cost by trajectory length & lowest score to not overweight importance
+  // This is a preferential cost, not collision cost, to be tuned relative to desired behaviors
+  auto && repulsive_cost_normalized =
+    (repulsive_cost - xt::amin(repulsive_cost, immediate)) / traj_len;
+
   data.costs += xt::pow(
     (critical_weight_ * raw_cost) +
-    (repulsion_weight_ * repulsive_cost / traj_len),
+    (repulsion_weight_ * repulsive_cost_normalized),
     power_);
   data.fail_flag = all_trajectories_collide;
 }

diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -36,13 +36,13 @@ void PathAngleCritic::initialize()
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(offset_from_furthest_, "offset_from_furthest", 4);
   getParam(power_, "cost_power", 1);
-  getParam(weight_, "cost_weight", 2.0);
+  getParam(weight_, "cost_weight", 2.2);
   getParam(
     threshold_to_consider_,
     "threshold_to_consider", 0.5);
   getParam(
     max_angle_to_furthest_,
-    "max_angle_to_furthest", 1.2);
+    "max_angle_to_furthest", 0.785398);
 
   int mode = 0;
   getParam(mode, "mode", mode);

diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
@@ -282,7 +282,7 @@ TEST(CriticTests, PathAngleCritic)
   path.y(6) = 4.0;
   critic.score(data);
   EXPECT_GT(xt::sum(costs, immediate)(), 0.0);
-  EXPECT_NEAR(costs(0), 3.6315, 1e-2);  // atan2(4,-1) [1.81] * 2.0 weight
+  EXPECT_NEAR(costs(0), 3.9947, 1e-2);  // atan2(4,-1) [1.81] * 2.2 weight
 
   // Set mode to no directional preferences + reset costs
   critic.setMode(1);
@@ -306,7 +306,7 @@ TEST(CriticTests, PathAngleCritic)
   critic.score(data);
   EXPECT_GT(xt::sum(costs, immediate)(), 0.0);
   // should use reverse orientation as the closer angle in no dir preference mode
-  EXPECT_NEAR(costs(0), 2.6516, 1e-2);
+  EXPECT_NEAR(costs(0), 2.9167, 1e-2);
 
   // Set mode to consider path directionality + reset costs
   critic.setMode(2);
@@ -330,7 +330,7 @@ TEST(CriticTests, PathAngleCritic)
   critic.score(data);
   EXPECT_GT(xt::sum(costs, immediate)(), 0.0);
   // should use reverse orientation as the closer angle in no dir preference mode
-  EXPECT_NEAR(costs(0), 2.6516, 1e-2);
+  EXPECT_NEAR(costs(0), 2.9167, 1e-2);
 
   PathAngleMode mode;
   mode = PathAngleMode::FORWARD_PREFERENCE;