Merge branch 'main' into main

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: SakshayMahna

.circleci/config.yml

@@ -33,12 +33,13 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v33\
+              - "<< parameters.key >>-v35\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v33\
+
+              - "<< parameters.key >>-v35\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +59,8 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v33\
+
+            key: "<< parameters.key >>-v35\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

.github/mergify.yml

@@ -26,6 +26,15 @@ pull_request_rules:
         branches:
           - humble
 
+  - name: backport to kilted at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-kilted"
+    actions:
+      backport:
+        branches:
+          - kilted
+
   - name: delete head branch after merge
     conditions:
       - merged

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_controller/src/controller_server.cpp

@@ -68,7 +68,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()},
-    get_parameter("use_sim_time").as_bool());
+    get_parameter("use_sim_time").as_bool(), options);
 }
 
 ControllerServer::~ControllerServer()

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -91,7 +91,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   explicit Costmap2DROS(
     const std::string & name,
     const std::string & parent_namespace = "/",
-    const bool & use_sim_time = false);
+    const bool & use_sim_time = false,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   /**
    * @brief Common initialization for constructors
@@ -430,6 +431,27 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 };
 
+// free functions
+
+/**
+  * @brief Given a specified argument name, replaces it with the specified
+  * new value. If the argument is not in the existing list, a new argument
+  * is created with the specified option.
+  */
+void replaceOrAddArgument(
+  std::vector<std::string> & arguments, const std::string & option,
+  const std::string & arg_name, const std::string & new_argument);
+
+/**
+  * @brief Given the node options of a parent node, expands of replaces
+  *         the fields for the node name, namespace and use_sim_time
+  */
+rclcpp::NodeOptions getChildNodeOptions(
+  const std::string & name,
+  const std::string & parent_namespace,
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & parent_options);
+
 }  // namespace nav2_costmap_2d
 
 #endif  // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_

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_costmap_2d/src/costmap_2d_ros.cpp

@@ -71,21 +71,48 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options)
   init();
 }
 
+void replaceOrAddArgument(
+  std::vector<std::string> & arguments, const std::string & option,
+  const std::string & arg_name, const std::string & new_argument)
+{
+  auto argument = std::find_if(arguments.begin(), arguments.end(),
+      [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;});
+  if (argument != arguments.end()) {
+    *argument = new_argument;
+  } else {
+    arguments.push_back("--ros-args");
+    arguments.push_back(option);
+    arguments.push_back(new_argument);
+  }
+}
+
+rclcpp::NodeOptions getChildNodeOptions(
+  const std::string & name,
+  const std::string & parent_namespace,
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & parent_options)
+{
+  std::vector<std::string> new_arguments = parent_options.arguments();
+  replaceOrAddArgument(new_arguments, "-r", "__ns",
+      "__ns:=" + nav2_util::add_namespaces(parent_namespace, name));
+  replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name);
+  replaceOrAddArgument(new_arguments, "-p", "use_sim_time",
+      "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"));
+  return rclcpp::NodeOptions().arguments(new_arguments);
+}
+
 Costmap2DROS::Costmap2DROS(
   const std::string & name,
   const std::string & parent_namespace,
-  const bool & use_sim_time)
+  const bool & use_sim_time,
+  const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode(name, "",
     // NodeOption arguments take precedence over the ones provided on the command line
     // use this to make sure the node is placed on the provided namespace
     // TODO(orduno) Pass a sub-node instead of creating a new node for better handling
     //              of the namespaces
-    rclcpp::NodeOptions().arguments({
-    "--ros-args", "-r", std::string("__ns:=") +
-    nav2_util::add_namespaces(parent_namespace, name),
-    "--ros-args", "-r", name + ":" + std::string("__node:=") + name,
-    "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"),
-  })),
+    getChildNodeOptions(name, parent_namespace, use_sim_time, options)
+),
   name_(name),
   default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
   default_types_{