Humble release 12: August 23 (#4644)

* Add configure and cleanup transitions to lifecycle manager and client (#4371)

Signed-off-by: Joni Pöllänen <[email protected]>

* [RotationShimController] Rotate to goal heading (#4332)

When arriving in the goal xy tolerance, the rotation shim controller
takes back the control to command the robot to rotate in the goal
heading orientation.

The initial goal of the rotationShimController was to rotate the robot
at the beginning of a navigation towards the paths orientation because
some controllers are not good at performing in place rotations. For the
same reason, the rotationShimController should be able to rotate the
robot towards the goal heading.

Signed-off-by: Antoine Gennart <[email protected]>

* [RotationShimController] Fix test for rotate to goal heading (#4289) (#4391)

* Fix rotate to goal heading tests

Signed-off-by: Antoine Gennart <[email protected]>

* reset laser_scan_filter before reinit (#4397)

Signed-off-by: goes <[email protected]>
Co-authored-by: goes <[email protected]>

* Warn if inflation_radius_ < inscribed_radius_ (#4423)

* Warn if inflation_radius_ < inscribed_radius_

Signed-off-by: Tony Najjar <[email protected]>

* convert to error

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>

* chore: cleanup ros1 leftovers (#4446)

Signed-off-by: Rein Appeldoorn <[email protected]>

* precomputeDistanceHeuristic is now computed once (#4451)

Signed-off-by: Vincent Belpois <[email protected]>
Co-authored-by: SiddharthaUpase <[email protected]>

* shutdown services in destructor of `ClearCostmapService` (#4495)

Signed-off-by: GoesM_server <[email protected]>
Co-authored-by: GoesM_server <[email protected]>

* fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507)

Signed-off-by: Kemal Bektas <[email protected]>
Co-authored-by: Kemal Bektas <[email protected]>

* min_turning_r_ getting param fix (#4510)

* min_turning_r_ getting param fix

Signed-off-by: Ivan Radionov <[email protected]>

* Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Ivan Radionov <[email protected]>

---------

Signed-off-by: Ivan Radionov <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Ivan Radionov <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

* Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517)

Signed-off-by: Joey Yang <[email protected]>

* check nullptr in smoothPlan() (#4544)

* check nullptr in smoothPlan()

Signed-off-by: GoesM <[email protected]>

* code-style

Signed-off-by: GoesM <[email protected]>

* code-style

Signed-off-by: GoesM <[email protected]>

* simple change

Signed-off-by: GoesM <[email protected]>

---------

Signed-off-by: GoesM <[email protected]>
Co-authored-by: GoesM <[email protected]>

* bump to 1.1.15

Signed-off-by: Steve Macenski <[email protected]>

* Revert "Add configure and cleanup transitions to lifecycle manager and client (#4371)"

This reverts commit 06ec958.

* fix merge conflict with humble sync

* fix merge conflict with humble sync

---------

Signed-off-by: Joni Pöllänen <[email protected]>
Signed-off-by: Antoine Gennart <[email protected]>
Signed-off-by: Antoine Gennart <[email protected]>
Signed-off-by: goes <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Rein Appeldoorn <[email protected]>
Signed-off-by: Vincent Belpois <[email protected]>
Signed-off-by: GoesM_server <[email protected]>
Signed-off-by: Kemal Bektas <[email protected]>
Signed-off-by: Ivan Radionov <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Joey Yang <[email protected]>
Signed-off-by: GoesM <[email protected]>
Co-authored-by: Joni Pöllänen <[email protected]>
Co-authored-by: Saitama <[email protected]>
Co-authored-by: GoesM <[email protected]>
Co-authored-by: goes <[email protected]>
Co-authored-by: Tony Najjar <[email protected]>
Co-authored-by: Rein Appeldoorn <[email protected]>
Co-authored-by: Vincent <[email protected]>
Co-authored-by: SiddharthaUpase <[email protected]>
Co-authored-by: Kemal Bektas <[email protected]>
Co-authored-by: Kemal Bektas <[email protected]>
Co-authored-by: Ivan Radionov <[email protected]>
Co-authored-by: Ivan Radionov <[email protected]>
Co-authored-by: Joey Yang <[email protected]>

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.1.15</version>
+  <version>1.1.16</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/amcl_node.cpp

@@ -1358,6 +1358,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/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.1.15</version>
+  <version>1.1.16</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>TODO</description>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>Collision Monitor</description>
   <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

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.1.15</version>
+  <version>1.1.16</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">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.1.15</version>
+  <version>1.1.16</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;

nav2_costmap_2d/src/clear_costmap_service.cpp

@@ -59,6 +59,14 @@ ClearCostmapService::ClearCostmapService(
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 }
 
+ClearCostmapService::~ClearCostmapService()
+{
+  // make sure services shutdown.
+  clear_except_service_.reset();
+  clear_around_service_.reset();
+  clear_entire_service_.reset();
+}
+
 void ClearCostmapService::clearExceptRegionCallback(
   const shared_ptr<rmw_request_id_t>/*request_header*/,
   const shared_ptr<ClearExceptRegion::Request> request,

nav2_dwb_controller/costmap_queue/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.1.15</version>
+  <version>1.1.16</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_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>dwb_core</name>
-  <version>1.1.15</version>
+  <version>1.1.16</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_critics/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.1.15</version>
+  <version>1.1.16</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_msgs/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>dwb_msgs</name>
-  <version>1.1.15</version>
+  <version>1.1.16</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>