diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 8801d98d6b..763a601ed8 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/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 param_subscriber_; - void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event); - // Message filters void initMessageFilters(); std::unique_ptr> laser_scan_sub_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 3c24cc5b11..e857cf648d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/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(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>( + rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data); + laser_scan_filter_ = std::make_unique>( *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>( - 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::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, diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 943871a319..c139f28b65 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/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 padded_footprint_; std::unique_ptr clear_costmap_service_; - - std::shared_ptr param_subscriber_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 49f21ebe44..aeb4feff3a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -169,8 +169,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_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 6cd9f3ac3d..c56cfd8553 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/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 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() {} @@ -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 param_subscriber_; - std::vector 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 local_params_; - std::recursive_mutex mutex_; - private: std::vector footprint_spec_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 69816f5e92..66648bf6f6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -160,8 +160,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 diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 41ade34c81..764496cc68 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/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, diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 10988cda84..7206334c91 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/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 lock(mutex_); - enabled_ = p.get_value(); - need_reinflation_ = true; - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".inflate_unknown", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - inflate_unknown_ = p.get_value(); - need_reinflation_ = true; - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".cost_scaling_factor", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - cost_scaling_factor_ = p.get_value(); - need_reinflation_ = true; - computeCaches(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".inflation_radius", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - inflation_radius_ = p.get_value(); - cell_inflation_radius_ = cellDistance(inflation_radius_); - need_reinflation_ = true; - computeCaches(); - })); - } } void diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 3ab2e7da16..e40b02039c 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/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 lock(mutex_); - enabled_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".footprint_clearing_enabled", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - footprint_clearing_enabled_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".max_obstacle_height", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - max_obstacle_height_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".combination_method", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - combination_method_ = p.get_value(); - })); - } } void diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index aa775208c3..2ae23e6a87 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/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()) { - enabled_ = p.get_value(); - 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 diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 0b3afabf50..24f2b6faed 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/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 lock(mutex_); - enabled_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".footprint_clearing_enabled", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - footprint_clearing_enabled_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".max_obstacle_height", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - max_obstacle_height_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".combination_method", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - combination_method_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - size_z_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - origin_z_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - z_resolution_ = p.get_value(); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + - ".unknown_threshold", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - unknown_threshold_ = p.get_value() + (VOXEL_BITS - size_z_); - })); - callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold", - [&](const rclcpp::Parameter & p) { - std::lock_guard lock(mutex_); - mark_threshold_ = p.get_value(); - })); - } } VoxelLayer::~VoxelLayer() diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 29e8fa3000..0ca175d47b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -137,8 +137,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - param_subscriber_ = std::make_shared(shared_from_this()); - // Then load and add the plug-ins to the costmap for (unsigned int i = 0; i < plugin_names_.size(); ++i) { RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str()); @@ -148,7 +146,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // TODO(mjeronimo): instead of get(), use a shared ptr plugin->initialize(layered_costmap_, plugin_names_[i], tf_buffer_.get(), - shared_from_this(), client_node_, rclcpp_node_, param_subscriber_); + shared_from_this(), client_node_, rclcpp_node_); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } @@ -177,8 +175,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // Add cleaning service clear_costmap_service_ = std::make_unique(shared_from_this(), *this); - param_subscriber_->set_event_callback( - std::bind(&Costmap2DROS::paramEventCallback, this, std::placeholders::_1)); return nav2_util::CallbackReturn::SUCCESS; } @@ -283,72 +279,6 @@ Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &) return nav2_util::CallbackReturn::SUCCESS; } -void -Costmap2DROS::paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/) -{ - if (map_update_thread_ != NULL) { - map_update_thread_shutdown_ = true; - map_update_thread_->join(); - delete map_update_thread_; - } - map_update_thread_shutdown_ = false; - - get_parameter("transform_tolerance", transform_tolerance_); - get_parameter("map_update_frequency", map_update_frequency_); - get_parameter("map_publish_frequency", map_publish_frequency_); - if (map_publish_frequency_ > 0) { - publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); - } else { - publish_cycle_ = rclcpp::Duration(-1); - } - - get_parameter("width", map_width_meters_); - get_parameter("height", map_height_meters_); - get_parameter("resolution", resolution_); - get_parameter("origin_x", origin_x_); - get_parameter("origin_y", origin_y_); - if (!layered_costmap_->isSizeLocked()) { - layered_costmap_->resizeMap((unsigned int)(map_width_meters_ / resolution_), - (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_); - } - - double footprint_padding; - get_parameter("footprint_padding", footprint_padding); - if (footprint_padding_ != footprint_padding) { - footprint_padding_ = footprint_padding; - setRobotFootprint(unpadded_footprint_); - } - - std::string footprint; - double robot_radius; - get_parameter("footprint", footprint); - get_parameter("robot_radius", robot_radius); - if (footprint_ != footprint || robot_radius_ != robot_radius) { - footprint_ = footprint; - robot_radius_ = robot_radius; - use_radius_ = true; - if (footprint_ != "" && footprint_ != "[]") { - std::vector new_footprint; - if (makeFootprintFromString(footprint_, new_footprint)) { - use_radius_ = false; - } else { - RCLCPP_ERROR( - get_logger(), "The footprint parameter is invalid: \"%s\", using radius (%lf) instead", - footprint_.c_str(), robot_radius_); - } - } - if (use_radius_) { - setRobotFootprint(makeFootprintFromRadius(robot_radius_)); - } else { - std::vector new_footprint; - makeFootprintFromString(footprint_, new_footprint); - setRobotFootprint(new_footprint); - } - } - map_update_thread_ = new std::thread(std::bind( - &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_)); -} - void Costmap2DROS::getParameters() { diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index c51e43f8db..228c060f70 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -49,8 +49,7 @@ Layer::initialize( LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf, nav2_util::LifecycleNode::SharedPtr node, rclcpp::Node::SharedPtr client_node, - rclcpp::Node::SharedPtr rclcpp_node, - std::shared_ptr param_subscriber) + rclcpp::Node::SharedPtr rclcpp_node) { layered_costmap_ = parent; name_ = name; @@ -58,7 +57,6 @@ Layer::initialize( node_ = node; client_node_ = client_node; rclcpp_node_ = rclcpp_node; - param_subscriber_ = param_subscriber; onInitialize(); } diff --git a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp deleted file mode 100644 index 3332940071..0000000000 --- a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp +++ /dev/null @@ -1,239 +0,0 @@ -// Copyright 2019 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ -#define NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/parameter_events_filter.hpp" - -namespace nav2_util -{ - -struct ParameterEventsCallbackHandle -{ - RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle) - - using ParameterEventsCallbackType = std::function; - - std::string parameter_name; - std::string node_name; - ParameterEventsCallbackType callback; -}; - -class ParameterEventsSubscriber -{ -public: - ParameterEventsSubscriber( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS()); - - template - ParameterEventsSubscriber( - NodeT node, - const rclcpp::QoS & qos = - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) - : ParameterEventsSubscriber( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - qos) - {} - - /// Set a custom callback for parameter events. - /** - * If no namespace is provided, a subscription will be created for the current namespace. - * Repeated calls to this function will overwrite the callback. - * If more than one namespace already has a subscription to its parameter events topic, then the - * provided callback will be applied to all of them. - * - * \param[in] callback Function callback to be evaluated on event. - * \param[in] node_namespaces Vector of namespaces for which a subscription will be created. - */ - void - set_event_callback( - std::function callback, - const std::vector & node_namespaces = {""}); - - /// Remove parameter event callback. - /** - * Calling this function will set the event callback to nullptr. This function will also remove - * event subscriptions on the namespaces for which there are no other callbacks (from parameter - * callbacks) active on that namespace. - */ - void - remove_event_callback(); - - using ParameterEventsCallbackType = ParameterEventsCallbackHandle::ParameterEventsCallbackType; - - /// Add a custom callback for a specified parameter. - /** - * If a node_name is not provided, defaults to the current node. - * - * \param[in] parameter_name Name of parameter. - * \param[in] callback Function callback to be evaluated upon parameter event. - * \param[in] node_name Name of node which hosts the parameter. - */ - ParameterEventsCallbackHandle::SharedPtr - add_parameter_callback( - const std::string & parameter_name, - ParameterEventsCallbackType callback, - const std::string & node_name = ""); - - /// Remove a custom callback for a specified parameter given its callback handle. - /** - * The parameter name and node name are inspected from the callback handle. The callback handle - * is erased from the list of callback handles on the {parameter_name, node_name} in the map. - * An error is thrown if the handle does not exist and/or was already removed. - * - * \param[in] handle Pointer to callback handle to be removed. - */ - void - remove_parameter_callback( - const ParameterEventsCallbackHandle * const handle); - - /// Remove a custom callback for a specified parameter given its name and respective node. - /** - * If a node_name is not provided, defaults to the current node. - * The {parameter_name, node_name} key on the parameter_callbacks_ map will be erased, removing - * all callback associated with that parameter. - * - * \param[in] parameter_name Name of parameter. - * \param[in] node_name Name of node which hosts the parameter. - */ - void - remove_parameter_callback( - const std::string & parameter_name, - const std::string & node_name = ""); - - /// Get a rclcpp::Parameter from parameter event, return true if parameter name & node in event. - /** - * If a node_name is not provided, defaults to the current node. - * - * \param[in] event Event msg to be inspected. - * \param[out] parameter Reference to rclcpp::Parameter to be assigned. - * \param[in] parameter_name Name of parameter. - * \param[in] node_name Name of node which hosts the parameter. - * \returns true if requested parameter name & node is in event, false otherwise - */ - static bool - get_parameter_from_event( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event, - rclcpp::Parameter & parameter, - const std::string parameter_name, - const std::string node_name = ""); - - /// Get a rclcpp::Parameter from parameter event - /** - * If a node_name is not provided, defaults to the current node. - * - * The user is responsible to check if the returned parameter has been properly assigned. - * By default, if the requested parameter is not found in the event, the returned parameter - * has parameter value of type rclcpp::PARAMETER_NOT_SET. - * - * \param[in] event Event msg to be inspected. - * \param[in] parameter_name Name of parameter. - * \param[in] node_name Name of node which hosts the parameter. - * \returns The resultant rclcpp::Parameter from the event - */ - static rclcpp::Parameter - get_parameter_from_event( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event, - const std::string parameter_name, - const std::string node_name = ""); - - using CallbacksContainerType = std::list; - -protected: - /// Add a subscription (if unique) to a namespace parameter events topic. - void - add_namespace_event_subscriber(const std::string & node_namespace); - - /// Remove a subscription to a namespace parameter events topic. - void - remove_namespace_event_subscriber(const std::string & node_namespace); - - /// Return true if any callbacks still exist on a namespace event topic, otherwise return false - bool - should_unsubscribe_to_namespace(const std::string & node_namespace); - - /// Callback for parameter events subscriptions. - void - event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); - - // Utility functions for string and path name operations. - std::string resolve_path(const std::string & path); - std::pair split_path(const std::string & str); - std::string join_path(std::string path, std::string name); - - // Node Interfaces used for logging and creating subscribers. - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - - rclcpp::QoS qos_; - - // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels - // Hash function for string pair required in std::unordered_map - class StringPairHash - { - public: - template - inline void hash_combine(std::size_t & seed, const T & v) const - { - std::hash hasher; - seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - - inline size_t operator()(const std::pair & s) const - { - size_t seed = 0; - hash_combine(seed, s.first); - hash_combine(seed, s.second); - return seed; - } - }; - // *INDENT-ON* - - // Map container for registered parameters. - std::unordered_map< - std::pair, - CallbacksContainerType, - StringPairHash - > parameter_callbacks_; - - // Vector of unique namespaces added. - std::vector subscribed_namespaces_; - // Vector of event callback namespaces - std::vector event_namespaces_; - - // Vector of event subscriptions for each namespace. - std::vector::SharedPtr> event_subscriptions_; - - std::function event_callback_; - - std::recursive_mutex mutex_; -}; - -} // namespace nav2_util - -#endif // NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index bf5aae60c5..70f1c7b624 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -7,7 +7,6 @@ add_library(${library_name} SHARED lifecycle_node.cpp robot_utils.cpp node_thread.cpp - parameter_events_subscriber.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/parameter_events_subscriber.cpp b/nav2_util/src/parameter_events_subscriber.cpp deleted file mode 100644 index 5d1b9c8053..0000000000 --- a/nav2_util/src/parameter_events_subscriber.cpp +++ /dev/null @@ -1,308 +0,0 @@ -// Copyright 2019 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include "nav2_util/parameter_events_subscriber.hpp" - -namespace nav2_util -{ - -ParameterEventsSubscriber::ParameterEventsSubscriber( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::QoS & qos) -: node_base_(node_base), - node_topics_(node_topics), - node_logging_(node_logging), - qos_(qos) -{} - -void -ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace) -{ - std::lock_guard lock(mutex_); - if (std::find(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), - node_namespace) == subscribed_namespaces_.end()) - { - subscribed_namespaces_.push_back(node_namespace); - auto topic = join_path(node_namespace, "parameter_events"); - RCLCPP_DEBUG(node_logging_->get_logger(), "Subscribing to topic: %s", topic.c_str()); - - auto event_sub = rclcpp::create_subscription( - node_topics_, topic, qos_, - std::bind(&ParameterEventsSubscriber::event_callback, this, std::placeholders::_1)); - - event_subscriptions_.push_back(event_sub); - } -} - -void -ParameterEventsSubscriber::remove_namespace_event_subscriber(const std::string & node_namespace) -{ - std::lock_guard lock(mutex_); - auto check_sub_cb = [this, &node_namespace]( - rclcpp::Subscription::SharedPtr sub) { - return sub->get_topic_name() == join_path(node_namespace, "parameter_events"); - }; - - auto it = std::find_if( - event_subscriptions_.begin(), - event_subscriptions_.end(), - check_sub_cb); - - if (it != event_subscriptions_.end()) { - event_subscriptions_.erase(it); - subscribed_namespaces_.erase( - std::remove(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), node_namespace)); - } -} - -void -ParameterEventsSubscriber::set_event_callback( - std::function callback, - const std::vector & node_namespaces) -{ - std::lock_guard lock(mutex_); - remove_event_callback(); - std::string full_namespace; - for (auto & ns : node_namespaces) { - if (ns == "") { - full_namespace = node_base_->get_namespace(); - } else { - full_namespace = resolve_path(ns); - } - add_namespace_event_subscriber(full_namespace); - event_namespaces_.push_back(full_namespace); - } - - event_callback_ = callback; -} - -void -ParameterEventsSubscriber::remove_event_callback() -{ - std::lock_guard lock(mutex_); - // Clear current vector of event namespaces and remove subscriptions - auto temp_namespaces = event_namespaces_; - event_namespaces_.clear(); - for (auto temp_ns : temp_namespaces) { - if (should_unsubscribe_to_namespace(temp_ns)) { - remove_namespace_event_subscriber(temp_ns); - } - } - - event_callback_ = nullptr; -} - -ParameterEventsCallbackHandle::SharedPtr -ParameterEventsSubscriber::add_parameter_callback( - const std::string & parameter_name, - ParameterEventsCallbackType callback, - const std::string & node_name) -{ - std::lock_guard lock(mutex_); - auto full_node_name = resolve_path(node_name); - add_namespace_event_subscriber(split_path(full_node_name).first); - - auto handle = std::make_shared(); - handle->callback = callback; - handle->parameter_name = parameter_name; - handle->node_name = full_node_name; - // the last callback registered is executed first. - parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); - - return handle; -} - -struct HandleCompare - : public std::unary_function -{ - explicit HandleCompare(const ParameterEventsCallbackHandle * const base) - : base_(base) {} - bool operator()(const ParameterEventsCallbackHandle::WeakPtr & handle) - { - auto shared_handle = handle.lock(); - if (base_ == shared_handle.get()) { - return true; - } - return false; - } - const ParameterEventsCallbackHandle * const base_; -}; - -void -ParameterEventsSubscriber::remove_parameter_callback( - const ParameterEventsCallbackHandle * const handle) -{ - std::lock_guard lock(mutex_); - auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}]; - auto it = std::find_if( - container.begin(), - container.end(), - HandleCompare(handle)); - if (it != container.end()) { - container.erase(it); - if (container.empty()) { - parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); - if (should_unsubscribe_to_namespace(split_path(handle->node_name).first)) { - remove_namespace_event_subscriber(split_path(handle->node_name).first); - } - } - } else { - throw std::runtime_error("Callback doesn't exist"); - } -} - -bool -ParameterEventsSubscriber::should_unsubscribe_to_namespace(const std::string & node_namespace) -{ - auto it1 = std::find(event_namespaces_.begin(), event_namespaces_.end(), node_namespace); - if (it1 != event_namespaces_.end()) { - return false; - } - - auto it2 = parameter_callbacks_.begin(); - while (it2 != parameter_callbacks_.end()) { - if (split_path(it2->first.second).first == node_namespace) { - return false; - } - ++it2; - } - return true; -} - -void -ParameterEventsSubscriber::remove_parameter_callback( - const std::string & parameter_name, - const std::string & node_name) -{ - std::lock_guard lock(mutex_); - auto full_node_name = resolve_path(node_name); - parameter_callbacks_.erase({parameter_name, full_node_name}); - if (should_unsubscribe_to_namespace(split_path(full_node_name).first)) { - RCLCPP_INFO(node_logging_->get_logger(), "Removing namespace event subscription"); - remove_namespace_event_subscriber(split_path(full_node_name).first); - } -} - -bool -ParameterEventsSubscriber::get_parameter_from_event( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event, - rclcpp::Parameter & parameter, - const std::string parameter_name, - const std::string node_name) -{ - if (event->node == node_name) { - rclcpp::ParameterEventsFilter filter(event, {parameter_name}, - {rclcpp::ParameterEventsFilter::EventType::NEW, - rclcpp::ParameterEventsFilter::EventType::CHANGED}); - if (!filter.get_events().empty()) { - const auto & events = filter.get_events(); - auto param_msg = events.back().second; - parameter = rclcpp::Parameter::from_parameter_msg(*param_msg); - return true; - } - } - return false; -} - -rclcpp::Parameter -ParameterEventsSubscriber::get_parameter_from_event( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event, - const std::string parameter_name, - const std::string node_name) -{ - rclcpp::Parameter p(parameter_name); - get_parameter_from_event(event, p, parameter_name, node_name); - return p; -} - -void -ParameterEventsSubscriber::event_callback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -{ - std::lock_guard lock(mutex_); - const std::string & node_name = event->node; - RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s", - node_name.c_str()); - - for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) { - rclcpp::Parameter p; - if (get_parameter_from_event(event, p, it->first.first, it->first.second)) { - for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) { - auto shared_handle = cb->lock(); - if (nullptr != shared_handle) { - shared_handle->callback(p); - } else { - cb = it->second.erase(cb); - } - } - } - } - - if (event_callback_) { - event_callback_(event); - } -} - -std::string -ParameterEventsSubscriber::resolve_path(const std::string & path) -{ - std::string full_path; - - if (path == "") { - full_path = node_base_->get_fully_qualified_name(); - } else { - full_path = path; - if (*full_path.begin() != '/') { - full_path = join_path(node_base_->get_namespace(), full_path); - } - } - - return full_path; -} - -std::pair -ParameterEventsSubscriber::split_path(const std::string & str) -{ - std::string path; - std::size_t found = str.find_last_of("/\\"); - if (found == 0) { - path = str.substr(0, found + 1); - } else { - path = str.substr(0, found); - } - std::string name = str.substr(found + 1); - return {path, name}; -} - -std::string -ParameterEventsSubscriber::join_path(std::string path, std::string name) -{ - std::string joined_path = path; - if (*joined_path.rbegin() != '/' && *name.begin() != '/') { - joined_path = joined_path + "/"; - } - - return joined_path + name; -} - -} // namespace nav2_util