Merge branch 'kilted' of github.com:ros-navigation/navigation2 into kilted

Author: SteveMacenski

nav2_bringup/launch/tb3_loopback_simulation_launch.py

@@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription:
             'use_composition': use_composition,
             'use_respawn': use_respawn,
             'use_localization': 'False',  # Don't use SLAM, AMCL
+            'use_keepout_zones': 'False',
+            'use_speed_zones': 'False',
         }.items(),
     )
 

nav2_bringup/launch/tb3_simulation_launch.py

@@ -195,6 +195,8 @@ def generate_launch_description() -> LaunchDescription:
             'autostart': autostart,
             'use_composition': use_composition,
             'use_respawn': use_respawn,
+            'use_keepout_zones': 'False',
+            'use_speed_zones': 'False',
         }.items(),
     )
     # The SDF file for the world is a xacro file because we wanted to

nav2_bringup/params/nav2_params.yaml

@@ -229,6 +229,8 @@ local_costmap:
         plugin: "nav2_costmap_2d::KeepoutFilter"
         enabled: KEEPOUT_ZONE_ENABLED
         filter_info_topic: "keepout_costmap_filter_info"
+        override_lethal_cost: True
+        lethal_override_cost: 200
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
@@ -281,6 +283,8 @@ global_costmap:
         plugin: "nav2_costmap_2d::KeepoutFilter"
         enabled: KEEPOUT_ZONE_ENABLED
         filter_info_topic: "keepout_costmap_filter_info"
+        override_lethal_cost: True
+        lethal_override_cost: 200
       speed_filter:
         plugin: "nav2_costmap_2d::SpeedFilter"
         enabled: SPEED_ZONE_ENABLED

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp

@@ -102,6 +102,12 @@ class KeepoutFilter : public CostmapFilter
   nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;
 
   std::string global_frame_;  // Frame of current layer (master_grid)
+
+  bool override_lethal_cost_{false};  // If true, lethal cost will be overridden
+  unsigned char lethal_override_cost_{252};  // Value to override lethal cost with
+  bool last_pose_lethal_{false};  // If true, last pose was lethal
+  unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u};
+  unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u};
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -74,6 +74,15 @@ void KeepoutFilter::initializeFilter(
     std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
+
+  declareParameter("override_lethal_cost", rclcpp::ParameterValue(false));
+  node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_);
+  declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE));
+  node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_);
+
+  // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given
+  lethal_override_cost_ = \
+    std::clamp<unsigned int>(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE);
 }
 
 void KeepoutFilter::filterInfoCallback(
@@ -149,7 +158,7 @@ void KeepoutFilter::maskCallback(
 void KeepoutFilter::process(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j,
-  const geometry_msgs::msg::Pose2D & /*pose*/)
+  const geometry_msgs::msg::Pose2D & pose)
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
@@ -244,10 +253,47 @@ void KeepoutFilter::process(
   }
 
   // unsigned<-signed conversions.
-  unsigned const int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
-  unsigned const int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
-  unsigned const int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
-  unsigned const int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
+  unsigned int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
+  unsigned int mg_min_y_u = static_cast<unsigned int>(mg_min_y);
+  unsigned int mg_max_x_u = static_cast<unsigned int>(mg_max_x);
+  unsigned int mg_max_y_u = static_cast<unsigned int>(mg_max_y);
+
+  // Let's find the pose's cost if we are allowed to override the lethal cost
+  bool is_pose_lethal = false;
+  if (override_lethal_cost_) {
+    geometry_msgs::msg::Pose2D mask_pose;
+    if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) {
+      unsigned int mask_robot_i, mask_robot_j;
+      if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) {
+        auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j);
+        is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE);
+        if (is_pose_lethal) {
+          RCLCPP_WARN_THROTTLE(
+            logger_, *(clock_), 2000,
+            "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out.");
+        }
+      }
+    }
+
+    // If in lethal space or just exited lethal space,
+    // we need to update all possible spaces touched during this state
+    if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) {
+      lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_);
+      mg_min_x_u = lethal_state_update_min_x_;
+      lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_);
+      mg_min_y_u = lethal_state_update_min_y_;
+      lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_);
+      mg_max_x_u = lethal_state_update_max_x_;
+      lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_);
+      mg_max_y_u = lethal_state_update_max_y_;
+    } else {
+      // If out of lethal space, reset managed lethal state sizes
+      lethal_state_update_min_x_ = master_grid.getSizeInCellsX();
+      lethal_state_update_min_y_ = master_grid.getSizeInCellsY();
+      lethal_state_update_max_x_ = 0u;
+      lethal_state_update_max_y_ = 0u;
+    }
+  }
 
   unsigned int i, j;  // master_grid iterators
   unsigned int index;  // corresponding index of master_grid
@@ -284,12 +330,19 @@ void KeepoutFilter::process(
         if (data == NO_INFORMATION) {
           continue;
         }
+
         if (data > old_data || old_data == NO_INFORMATION) {
-          master_array[index] = data;
+          if (override_lethal_cost_ && is_pose_lethal) {
+            master_array[index] = lethal_override_cost_;
+          } else {
+            master_array[index] = data;
+          }
         }
       }
     }
   }
+
+  last_pose_lethal_ = is_pose_lethal;
 }
 
 void KeepoutFilter::resetFilter()

nav2_loopback_sim/package.xml

@@ -12,6 +12,7 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>tf_transformations</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>python3-transforms3d</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -15,6 +15,10 @@
 #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
 
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/base/spaces/ReedsSheppStateSpace.h>
+
 #include <functional>
 #include <list>
 #include <memory>
@@ -61,7 +65,33 @@ class AnalyticExpansion
     Coordinates proposed_coords;
   };
 
-  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
+  /**
+   * @struct AnalyticExpansionNodes
+   * @brief Analytic expansion nodes and associated metadata
+   *
+   * This structure holds a collection of analytic expansion nodes and the number of direction
+   * changes encountered during the expansion.
+   */
+  struct AnalyticExpansionNodes
+  {
+    AnalyticExpansionNodes() = default;
+
+    void add(
+      NodePtr & node,
+      Coordinates & initial_coords,
+      Coordinates & proposed_coords)
+    {
+      nodes.emplace_back(node, initial_coords, proposed_coords);
+    }
+
+    void setDirectionChanges(int changes)
+    {
+      direction_changes = changes;
+    }
+
+    std::vector<AnalyticExpansionNode> nodes;
+    int direction_changes{0};
+  };
 
   /**
    * @brief Constructor for analytic expansion object
@@ -112,14 +142,15 @@ class AnalyticExpansion
 
   /**
    * @brief Refined analytic path from the current node to the goal
-   * @param current_node The node to start the analytic path from
+   * @param node The node to start the analytic path from. Node head may
+   * change as a result of refinement
    * @param goal_node The goal node to plan to
    * @param getter The function object that gets valid nodes from the graph
    * @param analytic_nodes The set of analytic nodes to refine
    * @return The score of the refined path
    */
   float refineAnalyticPath(
-    const NodePtr & current_node,
+    NodePtr & node,
     const NodePtr & goal_node,
     const NodeGetter & getter,
     AnalyticExpansionNodes & analytic_nodes);
@@ -135,6 +166,13 @@ class AnalyticExpansion
     const NodePtr & node, const NodePtr & goal,
     const AnalyticExpansionNodes & expanded_nodes);
 
+  /**
+    * @brief Counts the number of direction changes in a Reeds-Shepp path
+    * @param path The Reeds-Shepp path to count direction changes in
+    * @return The number of direction changes in the path
+    */
+  int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path);
+
   /**
    * @brief Takes an expanded nodes to clean up, if necessary, of any state
    * information that may be polluting it from a prior search iteration