cherry pick 2ac07b2 (#1417) for eloquent (#1641)

* cherry pick 2ac07b2 for eloquent

* remove a cherry pick error

Co-authored-by: Carl Delsey <carl.r.delsey@intel.com>

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -34,7 +34,6 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_amcl/motion_model/motion_model.hpp"
 #include "nav2_amcl/sensors/laser/laser.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
 #include "nav_msgs/srv/set_map.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
@@ -105,10 +104,6 @@ class AmclNode : public nav2_util::LifecycleNode
   tf2::Transform latest_tf_;
   void waitForTransforms();
 
-  // Parameter Event Subscriber
-  std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
-  void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event);
-
   // Message filters
   void initMessageFilters();
   std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;

nav2_amcl/src/amcl_node.cpp

@@ -194,21 +194,14 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Configuring");
 
   initParameters();
-  if (always_reset_initial_pose_) {
-    initial_pose_is_known_ = false;
-  }
   initTransforms();
-  initPubSub();
   initMessageFilters();
+  initPubSub();
   initServices();
   initOdometry();
   initParticleFilter();
   initLaserScan();
 
-  param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(shared_from_this());
-  param_subscriber_->set_event_callback(
-    std::bind(&AmclNode::parameterEventCallback, this, std::placeholders::_1));
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -350,15 +343,6 @@ AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
-void
-AmclNode::parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/)
-{
-  initParameters();
-  initMessageFilters();
-  initOdometry();
-  initParticleFilter();
-}
-
 void
 AmclNode::checkLaserReceived()
 {
@@ -1056,6 +1040,10 @@ AmclNode::initParameters()
       " this isn't allowed so max_particles will be set to min_particles.");
     max_particles_ = min_particles_;
   }
+
+  if (always_reset_initial_pose_) {
+    initial_pose_is_known_ = false;
+  }
 }
 
 void
@@ -1175,6 +1163,9 @@ AmclNode::initTransforms()
 void
 AmclNode::initMessageFilters()
 {
+  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
+    rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
+
   laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
     *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_);
 
@@ -1202,9 +1193,6 @@ AmclNode::initPubSub()
     std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
 
   RCLCPP_INFO(get_logger(), "Subscribed to map topic.");
-
-  laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
-    rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
 }
 
 void
@@ -1237,9 +1225,7 @@ AmclNode::initOdometry()
     init_cov_[1] = last_published_pose_.pose.covariance[7];
     init_cov_[2] = last_published_pose_.pose.covariance[35];
   }
-  if (motion_model_) {
-    motion_model_.reset();
-  }
+
   motion_model_ = std::unique_ptr<nav2_amcl::MotionModel>(nav2_amcl::MotionModel::createMotionModel(
         robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_));
 
@@ -1249,11 +1235,6 @@ AmclNode::initOdometry()
 void
 AmclNode::initParticleFilter()
 {
-  if (pf_ != nullptr) {
-    pf_free(pf_);
-    pf_ = nullptr;
-  }
-
   // Create the particle filter
   pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
       (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -50,7 +50,6 @@
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "tf2/convert.h"
 #include "tf2/LinearMath/Transform.h"
@@ -280,8 +279,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode
 
   // Parameters
   void getParameters();
-  void paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event);
-
   bool always_send_full_costmap_{false};
   std::string footprint_;
   float footprint_padding_{0};
@@ -307,8 +304,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
-
-  std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -168,8 +168,6 @@ class InflationLayer : public Layer
     unsigned int index, unsigned int mx, unsigned int my,
     unsigned int src_x, unsigned int src_y);
 
-  void setParamCallbacks();
-
   double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
   bool inflate_unknown_;
   unsigned int cell_inflation_radius_;

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -46,7 +46,6 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -65,8 +64,7 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
     tf2_ros::Buffer * tf,
     nav2_util::LifecycleNode::SharedPtr node,
     rclcpp::Node::SharedPtr client_node,
-    rclcpp::Node::SharedPtr rclcpp_node,
-    std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber = nullptr);
+    rclcpp::Node::SharedPtr rclcpp_node);
   virtual void deactivate() {} /** @brief Stop publishers. */
   virtual void activate() {}   /** @brief Restart publishers if they've been stopped. */
   virtual void reset() = 0;
@@ -137,8 +135,6 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   nav2_util::LifecycleNode::SharedPtr node_;
   rclcpp::Node::SharedPtr client_node_;
   rclcpp::Node::SharedPtr rclcpp_node_;
-  std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
-  std::vector<nav2_util::ParameterEventsCallbackHandle::SharedPtr> callback_handles_;
 
   /** @brief This is called at the end of initialize().  Override to
    * implement subclass-specific initialization.
@@ -155,8 +151,6 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   // Names of the parameters declared on the ROS node
   std::unordered_set<std::string> local_params_;
 
-  std::recursive_mutex mutex_;
-
 private:
   std::vector<geometry_msgs::msg::Point> footprint_spec_;
 };

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -164,8 +164,6 @@ class ObstacleLayer : public CostmapLayer
     double * max_x,
     double * max_y);
 
-  void setParamCallbacks();
-
   std::string global_frame_;  ///< @brief The global frame for the costmap
   double max_obstacle_height_;  ///< @brief Max Obstacle Height
 

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -85,7 +85,7 @@ class VoxelLayer : public ObstacleLayer
   virtual void resetMaps();
 
 private:
-  void setParamCallbacks();
+  void reconfigureCB();
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
   virtual void raytraceFreespace(
     const nav2_costmap_2d::Observation & clearing_observation,

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -75,8 +75,6 @@ InflationLayer::InflationLayer()
 void
 InflationLayer::onInitialize()
 {
-  RCLCPP_INFO(node_->get_logger(), "Initializing inflation layer!");
-
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
   declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
@@ -92,44 +90,6 @@ InflationLayer::onInitialize()
   need_reinflation_ = false;
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   matchSize();
-  setParamCallbacks();
-}
-
-void
-InflationLayer::setParamCallbacks()
-{
-  if (param_subscriber_) {
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        enabled_ = p.get_value<bool>();
-        need_reinflation_ = true;
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".inflate_unknown",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        inflate_unknown_ = p.get_value<bool>();
-        need_reinflation_ = true;
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".cost_scaling_factor",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        cost_scaling_factor_ = p.get_value<double>();
-        need_reinflation_ = true;
-        computeCaches();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".inflation_radius",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        inflation_radius_ = p.get_value<double>();
-        cell_inflation_radius_ = cellDistance(inflation_radius_);
-        need_reinflation_ = true;
-        computeCaches();
-      }));
-  }
 }
 
 void

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -243,37 +243,6 @@ void ObstacleLayer::onInitialize()
       observation_notifiers_.back()->setTargetFrames(target_frames);
     }
   }
-  setParamCallbacks();
-}
-
-void
-ObstacleLayer::setParamCallbacks()
-{
-  if (param_subscriber_) {
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        enabled_ = p.get_value<bool>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".footprint_clearing_enabled",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        footprint_clearing_enabled_ = p.get_value<bool>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".max_obstacle_height",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        max_obstacle_height_ = p.get_value<double>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".combination_method",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        combination_method_ = p.get_value<int>();
-      }));
-  }
 }
 
 void

nav2_costmap_2d/plugins/static_layer.cpp

@@ -94,19 +94,6 @@ StaticLayer::onInitialize()
       rclcpp::SystemDefaultsQoS(),
       std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
   }
-
-  auto cb = [&](const rclcpp::Parameter & p) {
-      if (enabled_ != p.get_value<bool>()) {
-        enabled_ = p.get_value<bool>();
-        has_updated_data_ = true;
-        x_ = y_ = 0;
-        width_ = size_x_;
-        height_ = size_y_;
-      }
-    };
-  if (param_subscriber_) {
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled", cb));
-  }
 }
 
 void

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -95,63 +95,6 @@ void VoxelLayer::onInitialize()
 
   unknown_threshold_ += (VOXEL_BITS - size_z_);
   matchSize();
-  setParamCallbacks();
-}
-
-void
-VoxelLayer::setParamCallbacks()
-{
-  if (param_subscriber_) {
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        enabled_ = p.get_value<bool>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".footprint_clearing_enabled",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        footprint_clearing_enabled_ = p.get_value<bool>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".max_obstacle_height",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        max_obstacle_height_ = p.get_value<double>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".combination_method",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        combination_method_ = p.get_value<int>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        size_z_ = p.get_value<int>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        origin_z_ = p.get_value<double>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        z_resolution_ = p.get_value<double>();
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
-      ".unknown_threshold",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        unknown_threshold_ = p.get_value<int>() + (VOXEL_BITS - size_z_);
-      }));
-    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold",
-      [&](const rclcpp::Parameter & p) {
-        std::lock_guard<std::recursive_mutex> lock(mutex_);
-        mark_threshold_ = p.get_value<int>();
-      }));
-  }
 }
 
 VoxelLayer::~VoxelLayer()