Iron Sync 8: August 23, 2024 (#4645)

* earlier `executor_thread_.reset()` in nav2_costmap_ros (#4385) * reorder cleanup_queue Signed-off-by: goes <GoesM@buaa.edu.cn> * codestyle Signed-off-by: goes <GoesM@buaa.edu.cn> * codestyle Signed-off-by: goes <GoesM@buaa.edu.cn> --------- Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn> * [RotationShimController] Fix test for rotate to goal heading (#4289) (#4391) * Fix rotate to goal heading tests Signed-off-by: Antoine Gennart <gennartan@disroot.org> * Update bt_service_node.hpp debug msg (#4398) Provide service_name instead of service_node_name for debugging bt_service_node Signed-off-by: João Britto <cbn.joao@gmail.com> * reset laser_scan_filter before reinit (#4397) Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn> * Fix error messages (#4411) Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at> * Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * convert to error Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * chore: cleanup ros1 leftovers (#4446) Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> * precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com> Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com> * shutdown services in destructor of `ClearCostmapService` (#4495) Signed-off-by: GoesM_server <GoesM@buaa.edu.cn> Co-authored-by: GoesM_server <GoesM@buaa.edu.cn> * Fix world to map coordinate conversion (#4506) Signed-off-by: HovorunBh <fipogh@gmail.com> * fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507) Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> * min_turning_r_ getting param fix (#4510) * min_turning_r_ getting param fix Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com> --------- Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517) Signed-off-by: Joey Yang <joeyyang.ai@gmail.com> * check nullptr in smoothPlan() (#4544) * check nullptr in smoothPlan() Signed-off-by: GoesM <goesm@buaa.edu.cn> * code-style Signed-off-by: GoesM <goesm@buaa.edu.cn> * code-style Signed-off-by: GoesM <goesm@buaa.edu.cn> * simple change Signed-off-by: GoesM <goesm@buaa.edu.cn> --------- Signed-off-by: GoesM <goesm@buaa.edu.cn> Co-authored-by: GoesM <goesm@buaa.edu.cn> * check nullPtr in `computeControl()` (#4548) Signed-off-by: goes <GoesM@buaa.edu.cn> Co-authored-by: goes <GoesM@buaa.edu.cn> * bumping to 1.2.10 for release * fixing merge conflict iron --------- Signed-off-by: goes <GoesM@buaa.edu.cn> Signed-off-by: Antoine Gennart <gennartan@disroot.org> Signed-off-by: João Britto <cbn.joao@gmail.com> Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com> Signed-off-by: GoesM_server <GoesM@buaa.edu.cn> Signed-off-by: HovorunBh <fipogh@gmail.com> Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Ivan Radionov <i.a.radionov@gmail.com.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Joey Yang <joeyyang.ai@gmail.com> Signed-off-by: GoesM <goesm@buaa.edu.cn> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes <GoesM@buaa.edu.cn> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: João Britto <cbn.joao@gmail.com> Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Rein Appeldoorn <reinzor@gmail.com> Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com> Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com> Co-authored-by: Bohdan <72872431+HovorunBh@users.noreply.github.com> Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com> Co-authored-by: Ivan Radionov <i.a.radionov@gmail.com.com> Co-authored-by: Joey Yang <joeyyang.ai@gmail.com>
ros-navigation · Aug 24, 2024 · ddd1b4d · ddd1b4d
1 parent 09f6572
commit ddd1b4d
Show file tree

Hide file tree

Showing 67 changed files with 128 additions and 249 deletions.
diff --git a/nav2_amcl/package.xml b/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

diff --git a/nav2_amcl/src/amcl_node.cpp b/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();

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/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(

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/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"));

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/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(

diff --git a/nav2_behavior_tree/package.xml b/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>

diff --git a/nav2_behaviors/package.xml b/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>

diff --git a/nav2_bringup/package.xml b/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>

diff --git a/nav2_bt_navigator/package.xml b/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>

diff --git a/nav2_collision_monitor/package.xml b/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>

diff --git a/nav2_common/package.xml b/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>

diff --git a/nav2_constrained_smoother/package.xml b/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>

diff --git a/nav2_controller/package.xml b/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>

diff --git a/nav2_controller/src/controller_server.cpp b/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();

diff --git a/nav2_core/package.xml b/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>

diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg
diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg
diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg
diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg
diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/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
    */

diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch
diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml
diff --git a/nav2_costmap_2d/package.xml b/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

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/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",

diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/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();
       }

diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/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;