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;