Iron Sync 8: August 23, 2024

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/amcl_node.cpp

@@ -1367,6 +1367,7 @@ AmclNode::dynamicParametersCallback(
     lasers_update_.clear();
     frame_to_laser_.clear();
     laser_scan_connection_.disconnect();
+    laser_scan_filter_.reset();
     laser_scan_sub_.reset();
 
     initMessageFilters();

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -100,7 +100,7 @@ class BtActionNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for %f s",
+        node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
         action_name.c_str(),
         wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -93,8 +93,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
-        action_name.c_str());
+        node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
+        action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(
               std::string("Action server ") + action_name +
               std::string(" not available"));

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -84,12 +84,12 @@ class BtServiceNode : public BT::ActionNodeBase
       service_name_.c_str());
     if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
-        service_node_name.c_str());
+        node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
+        service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(
               std::string(
                 "Service server %s not available",
-                service_node_name.c_str()));
+                service_name_.c_str()));
     }
 
     RCLCPP_DEBUG(

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_behaviors/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bt_navigator/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_collision_monitor/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_common/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_constrained_smoother/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_controller/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_controller/src/controller_server.cpp

@@ -436,31 +436,36 @@ void ControllerServer::computeControl()
   RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort.");
 
   try {
-    std::string c_name = action_server_->get_current_goal()->controller_id;
+    auto goal = action_server_->get_current_goal();
+    if (!goal) {
+      return;  //  goal would be nullptr if action_server_ is inactivate.
+    }
+
+    std::string c_name = goal->controller_id;
     std::string current_controller;
     if (findControllerId(c_name, current_controller)) {
       current_controller_ = current_controller;
     } else {
       throw nav2_core::InvalidController("Failed to find controller name: " + c_name);
     }
 
-    std::string gc_name = action_server_->get_current_goal()->goal_checker_id;
+    std::string gc_name = goal->goal_checker_id;
     std::string current_goal_checker;
     if (findGoalCheckerId(gc_name, current_goal_checker)) {
       current_goal_checker_ = current_goal_checker;
     } else {
       throw nav2_core::ControllerException("Failed to find goal checker name: " + gc_name);
     }
 
-    std::string pc_name = action_server_->get_current_goal()->progress_checker_id;
+    std::string pc_name = goal->progress_checker_id;
     std::string current_progress_checker;
     if (findProgressCheckerId(pc_name, current_progress_checker)) {
       current_progress_checker_ = current_progress_checker;
     } else {
       throw nav2_core::ControllerException("Failed to find progress checker name: " + pc_name);
     }
 
-    setPlannerPath(action_server_->get_current_goal()->path);
+    setPlannerPath(goal->path);
     progress_checkers_[current_progress_checker_]->reset();
 
     last_valid_cmd_time_ = now();

nav2_core/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -48,6 +48,11 @@ class ClearCostmapService
    */
   ClearCostmapService() = delete;
 
+  /**
+   * @brief A destructor
+   */
+  ~ClearCostmapService();
+
   /**
    * @brief Clears the region outside of a user-specified area reverting to the static map
    */

nav2_costmap_2d/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.2.9</version>
+  <version>1.2.10</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged()
   computeCaches();
   need_reinflation_ = true;
 
+  if (inflation_radius_ < inscribed_radius_) {
+    RCLCPP_ERROR(
+      logger_,
+      "The configured inflation radius (%.3f) is smaller than "
+      "the computed inscribed radius (%.3f) of your footprint, "
+      "it is highly recommended to set inflation radius to be at "
+      "least as big as the inscribed radius to avoid collisions",
+      inflation_radius_, inscribed_radius_);
+  }
+
   RCLCPP_DEBUG(
     logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu,"
     " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -304,8 +304,11 @@ ObstacleLayer::dynamicParametersCallback(
         max_obstacle_height_ = parameter.as_double();
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
-      if (param_name == name_ + "." + "enabled") {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
         enabled_ = parameter.as_bool();
+        if (enabled_) {
+          current_ = false;
+        }
       } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
         footprint_clearing_enabled_ = parameter.as_bool();
       }

nav2_costmap_2d/plugins/static_layer.cpp

@@ -318,6 +318,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u
       "StaticLayer: Map update ignored. Current map is in frame %s "
       "but update was in frame %s",
       map_frame_.c_str(), update->header.frame_id.c_str());
+    return;
   }
 
   unsigned int di = 0;