45% performance improvement in MPPI (ros-navigation#4174)

* 18.5% performance improvement in MPPI * adding in path angle update * path align improvements * adding views * more updates with some TODOs * massive improvements to cost critic * remove TODOs * removing some jitter * updates * use fabs * 1ms saved baby! * completed optimizations * remove TODO * linting * fixing test failure * indexing fix * fix bug * make MPPI default for Jazzy * Adding higher velocity limits --------- Signed-off-by: Steve Macenski <[email protected]>
Manos-G · Aug 1, 2024 · 2a4b8a7 · 2a4b8a7
1 parent 1111032
commit 2a4b8a7
Show file tree

Hide file tree

Showing 28 changed files with 583 additions and 703 deletions.
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
@@ -93,48 +93,90 @@ controller_server:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
       yaw_goal_tolerance: 0.25
-    # DWB parameters
     FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
 local_costmap:
   local_costmap:
@@ -288,8 +330,8 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
+    max_velocity: [0.5, 0.0, 2.0]
+    min_velocity: [-0.5, 0.0, -2.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
     odom_topic: "odom"

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
@@ -51,11 +51,11 @@ class FootprintCollisionChecker
   /**
    * @brief Find the footprint cost in oriented footprint
    */
-  double footprintCost(const Footprint footprint);
+  double footprintCost(const Footprint & footprint);
   /**
    * @brief Find the footprint cost a a post with an unoriented footprint
    */
-  double footprintCostAtPose(double x, double y, double theta, const Footprint footprint);
+  double footprintCostAtPose(double x, double y, double theta, const Footprint & footprint);
   /**
    * @brief Get the cost for a line segment
    */

diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp
@@ -45,7 +45,7 @@ FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
 }
 
 template<typename CostmapT>
-double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint footprint)
+double FootprintCollisionChecker<CostmapT>::footprintCost(const Footprint & footprint)
 {
   // now we really have to lay down the footprint in the costmap_ grid
   unsigned int x0, x1, y0, y1;
@@ -116,7 +116,7 @@ bool FootprintCollisionChecker<CostmapT>::worldToMap(
 template<typename CostmapT>
 double FootprintCollisionChecker<CostmapT>::pointCost(int x, int y) const
 {
-  return costmap_->getCost(x, y);
+  return static_cast<double>(costmap_->getCost(x, y));
 }
 
 template<typename CostmapT>
@@ -127,13 +127,14 @@ void FootprintCollisionChecker<CostmapT>::setCostmap(CostmapT costmap)
 
 template<typename CostmapT>
 double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
-  double x, double y, double theta, const Footprint footprint)
+  double x, double y, double theta, const Footprint & footprint)
 {
   double cos_th = cos(theta);
   double sin_th = sin(theta);
   Footprint oriented_footprint;
+  oriented_footprint.reserve(footprint.size());
+  geometry_msgs::msg::Point new_pt;
   for (unsigned int i = 0; i < footprint.size(); ++i) {
-    geometry_msgs::msg::Point new_pt;
     new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th);
     new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th);
     oriented_footprint.push_back(new_pt);

diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
@@ -12,8 +12,8 @@ set(XTENSOR_USE_XSIMD 1)
 # set(XTENSOR_DEFAULT_TRAVERSAL row_major)  # row_major, column_major
 
 find_package(ament_cmake REQUIRED)
-find_package(xtensor REQUIRED)
 find_package(xsimd REQUIRED)
+find_package(xtensor REQUIRED)
 
 include_directories(
   include
@@ -83,7 +83,6 @@ add_library(mppi_critics SHARED
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
   src/critics/path_align_critic.cpp
-  src/critics/path_align_legacy_critic.cpp
   src/critics/path_follow_critic.cpp
   src/critics/path_angle_critic.cpp
   src/critics/prefer_forward_critic.cpp

diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -125,6 +125,7 @@ Uses inflated costmap cost directly to avoid obstacles
  | critical_cost       | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.                                          |
  | 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.   
  | inflation_layer_name        | string    | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
+ | trajectory_point_step      | int | Default 2. Step of trajectory points to evaluate for costs since otherwise so dense represents multiple points for a single costmap cell.   |
 
 #### Path Align Critic
  | Parameter                  | Type   | Definition                                                                                                                         |
@@ -133,12 +134,10 @@ Uses inflated costmap cost directly to avoid obstacles
  | 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 align cost stops being considered                                    |
  | offset_from_furthest      | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading.  |
- | trajectory_point_step      | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.   |
+ | trajectory_point_step      | int | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.   |
  | max_path_occupancy_ratio   | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene.  |
  | use_path_orientations   | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false.  |
 
-Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023.
-
 #### Path Angle Critic
  | Parameter                 | Type   | Definition                                                                                                  |
  | ---------------           | ------ | ----------------------------------------------------------------------------------------------------------- |
@@ -221,6 +220,7 @@ controller_server:
         cost_power: 1
         cost_weight: 5.0
         threshold_to_consider: 0.5
+      # Option to replace Cost and use Obstacles instead
       # ObstaclesCritic:
       #   enabled: true
       #   cost_power: 1
@@ -238,13 +238,13 @@ controller_server:
         consider_footprint: true
         collision_cost: 1000000.0
         near_goal_distance: 1.0
-      PathAlignCritic:
+        trajectory_point_step: 2
       PathAlignCritic:
         enabled: true
         cost_power: 1
         cost_weight: 14.0
         max_path_occupancy_ratio: 0.05
-        trajectory_point_step: 3
+        trajectory_point_step: 4
         threshold_to_consider: 0.5
         offset_from_furthest: 20
         use_path_orientations: false

diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml
@@ -21,10 +21,6 @@
       <description>mppi critic for aligning to path</description>
     </class>
 
-    <class type="mppi::critics::PathAlignLegacyCritic" base_class_type="mppi::critics::CriticFunction">
-      <description>mppi critic for aligning to path (legacy)</description>
-    </class>
-
     <class type="mppi::critics::PathAngleCritic" base_class_type="mppi::critics::CriticFunction">
       <description>mppi critic for tracking the path in the correct heading</description>
     </class>

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
@@ -42,6 +42,7 @@ namespace mppi
 class CriticManager
 {
 public:
+  typedef std::vector<std::unique_ptr<critics::CriticFunction>> Critics;
   /**
     * @brief Constructor for mppi::CriticManager
     */
@@ -94,7 +95,7 @@ class CriticManager
   ParametersHandler * parameters_handler_;
   std::vector<std::string> critic_names_;
   std::unique_ptr<pluginlib::ClassLoader<critics::CriticFunction>> loader_;
-  std::vector<std::unique_ptr<critics::CriticFunction>> critics_;
+  Critics critics_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -56,15 +56,29 @@ class CostCritic : public CriticFunction
     * @param theta theta of pose
     * @return bool if in collision
     */
-  bool inCollision(float cost, float x, float y, float theta);
-
-  /**
-    * @brief cost at a robot pose
-    * @param x X of pose
-    * @param y Y of pose
-    * @return Collision information at pose
-    */
-  float costAtPose(float x, float y);
+  inline bool inCollision(float cost, float x, float y, float theta)
+  {
+    // If consider_footprint_ check footprint scort for collision
+    float score_cost = cost;
+    if (consider_footprint_ &&
+      (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
+    {
+      score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+          static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
+          costmap_ros_->getRobotFootprint()));
+    }
+
+    switch (static_cast<unsigned char>(score_cost)) {
+      case (nav2_costmap_2d::LETHAL_OBSTACLE):
+        return true;
+      case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE):
+        return consider_footprint_ ? false : true;
+      case (nav2_costmap_2d::NO_INFORMATION):
+        return is_tracking_unknown_ ? false : true;
+    }
+
+    return false;
+  }
 
   /**
     * @brief Find the min cost of the inflation decay function for which the robot MAY be
@@ -73,19 +87,57 @@ class CostCritic : public CriticFunction
     * @return double circumscribed cost, any higher than this and need to do full footprint collision checking
     * since some element of the robot could be in collision
     */
-  float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
+  inline float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
+
+  /**
+    * @brief An implementation of worldToMap fully using floats
+    * @param wx Float world X coord
+    * @param wy Float world Y coord
+    * @param mx unsigned int map X coord
+    * @param my unsigned into map Y coord
+    * @return if successsful
+    */
+  inline bool worldToMapFloat(float wx, float wy, unsigned int & mx, unsigned int & my) const
+  {
+    if (wx < origin_x_ || wy < origin_y_) {
+      return false;
+    }
+
+    mx = static_cast<unsigned int>((wx - origin_x_) / resolution_);
+    my = static_cast<unsigned int>((wy - origin_y_) / resolution_);
+
+    if (mx < size_x_ && my < size_y_) {
+      return true;
+    }
+    return false;
+  }
+
+  /**
+    * @brief A local implementation of getIndex
+    * @param mx unsigned int map X coord
+    * @param my unsigned into map Y coord
+    * @return Index
+    */
+  inline unsigned int getIndex(unsigned int mx, unsigned int my) const
+  {
+    return my * size_x_ + mx;
+  }
 
-protected:
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
   collision_checker_{nullptr};
   float possible_collision_cost_;
 
   bool consider_footprint_{true};
-  float circumscribed_radius_{0};
-  float circumscribed_cost_{0};
-  float collision_cost_{0};
-  float critical_cost_{0};
+  bool is_tracking_unknown_{true};
+  float circumscribed_radius_{0.0f};
+  float circumscribed_cost_{0.0f};
+  float collision_cost_{0.0f};
+  float critical_cost_{0.0f};
   float weight_{0};
+  unsigned int trajectory_point_step_;
+
+  float origin_x_, origin_y_, resolution_;
+  unsigned int size_x_, size_y_;
 
   float near_goal_distance_;
   std::string inflation_layer_name_;