From 18948c58b04ff25f60945f4658e9d5ece52b30d7 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Mon, 18 Nov 2019 13:42:41 -0800 Subject: [PATCH 1/4] add parameter event subscriber to nav2 lifecycle node --- .../include/nav2_util/lifecycle_node.hpp | 3 + .../nav2_util/parameter_events_subscriber.hpp | 239 ++++++++++++++ nav2_util/src/CMakeLists.txt | 1 + nav2_util/src/lifecycle_node.cpp | 2 + nav2_util/src/parameter_events_subscriber.cpp | 308 ++++++++++++++++++ 5 files changed, 553 insertions(+) create mode 100644 nav2_util/include/nav2_util/parameter_events_subscriber.hpp create mode 100644 nav2_util/src/parameter_events_subscriber.cpp diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 745d745447..b39e8a0964 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -20,6 +20,7 @@ #include #include "nav2_util/node_thread.hpp" +#include "nav2_util/parameter_events_subscriber.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -125,6 +126,8 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // When creating a local node, this class will launch a separate thread created to spin the node std::unique_ptr rclcpp_thread_; + + std::shared_ptr param_subscriber_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp new file mode 100644 index 0000000000..e15dd01549 --- /dev/null +++ b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp @@ -0,0 +1,239 @@ +// 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 70f1c7b624..bf5aae60c5 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -7,6 +7,7 @@ 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/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e6efb5f6a9..74216894a8 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -57,6 +57,8 @@ LifecycleNode::LifecycleNode( "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); rclcpp_thread_ = std::make_unique(rclcpp_node_); } + + param_subscriber_ = std::make_shared(shared_from_this()); } LifecycleNode::~LifecycleNode() diff --git a/nav2_util/src/parameter_events_subscriber.cpp b/nav2_util/src/parameter_events_subscriber.cpp new file mode 100644 index 0000000000..5d1b9c8053 --- /dev/null +++ b/nav2_util/src/parameter_events_subscriber.cpp @@ -0,0 +1,308 @@ +// 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 From e3dc600123ccc8894e9bd36d4cd46786e66da915 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Mon, 18 Nov 2019 21:40:59 -0800 Subject: [PATCH 2/4] ROS1 parity dynamic parameter callbacks for costmap_2d add lambda callbacks directly --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 5 ++ .../nav2_costmap_2d/inflation_layer.hpp | 3 + .../include/nav2_costmap_2d/layer.hpp | 6 +- .../nav2_costmap_2d/obstacle_layer.hpp | 2 + .../include/nav2_costmap_2d/voxel_layer.hpp | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 38 ++++++++++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 27 +++++++ nav2_costmap_2d/plugins/static_layer.cpp | 13 ++++ nav2_costmap_2d/plugins/voxel_layer.cpp | 48 +++++++++++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 72 ++++++++++++++++++- nav2_costmap_2d/src/layer.cpp | 4 +- .../include/nav2_util/lifecycle_node.hpp | 3 - .../nav2_util/parameter_events_subscriber.hpp | 10 +-- nav2_util/src/lifecycle_node.cpp | 2 - 14 files changed, 221 insertions(+), 14 deletions(-) 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 c139f28b65..943871a319 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,6 +50,7 @@ #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" @@ -279,6 +280,8 @@ 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}; @@ -304,6 +307,8 @@ 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 aeb4feff3a..090e3c8715 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -169,6 +169,8 @@ 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_; @@ -185,6 +187,7 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; + std::recursive_mutex mutex_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index c56cfd8553..c63fd8beea 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -46,6 +46,7 @@ #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 { @@ -64,7 +65,8 @@ 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); + rclcpp::Node::SharedPtr rclcpp_node, + nav2_util::ParameterEventsSubscriber * param_subscriber = nullptr); virtual void deactivate() {} /** @brief Stop publishers. */ virtual void activate() {} /** @brief Restart publishers if they've been stopped. */ virtual void reset() {} @@ -135,6 +137,8 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface nav2_util::LifecycleNode::SharedPtr node_; rclcpp::Node::SharedPtr client_node_; rclcpp::Node::SharedPtr rclcpp_node_; + nav2_util::ParameterEventsSubscriber * param_subscriber_; + std::vector callback_handles_; /** @brief This is called at the end of initialize(). Override to * implement subclass-specific initialization. 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 66648bf6f6..69816f5e92 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -160,6 +160,8 @@ 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 764496cc68..41ade34c81 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 reconfigureCB(); + void setParamCallbacks(); 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 7206334c91..931de69539 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -75,6 +75,8 @@ 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)); @@ -90,6 +92,42 @@ 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) { + enabled_ = p.get_value(); + need_reinflation_ = true; + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".inflate_unknown", + [&](const rclcpp::Parameter & p) { + 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 e40b02039c..5f234a5289 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -243,6 +243,33 @@ 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) { + enabled_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".footprint_clearing_enabled", + [&](const rclcpp::Parameter & p) { + footprint_clearing_enabled_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".max_obstacle_height", + [&](const rclcpp::Parameter & p) { + max_obstacle_height_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".combination_method", + [&](const rclcpp::Parameter & p) { + 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 2ae23e6a87..aa775208c3 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -94,6 +94,19 @@ 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 24f2b6faed..1924e336f6 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -95,6 +95,54 @@ 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) { + enabled_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".footprint_clearing_enabled", + [&](const rclcpp::Parameter & p) { + footprint_clearing_enabled_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".max_obstacle_height", + [&](const rclcpp::Parameter & p) { + max_obstacle_height_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".combination_method", + [&](const rclcpp::Parameter & p) { + combination_method_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels", + [&](const rclcpp::Parameter & p) { + size_z_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z", + [&](const rclcpp::Parameter & p) { + origin_z_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution", + [&](const rclcpp::Parameter & p) { + z_resolution_ = p.get_value(); + })); + callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + + ".unknown_threshold", + [&](const rclcpp::Parameter & p) { + 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) { + 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 c6789f0452..39dbf8c66c 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -137,6 +137,8 @@ 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()); @@ -146,7 +148,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_); + shared_from_this(), client_node_, rclcpp_node_, param_subscriber_.get()); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } @@ -175,6 +177,8 @@ 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; } @@ -279,6 +283,72 @@ 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 228c060f70..571b4a5b18 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -49,7 +49,8 @@ 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) + rclcpp::Node::SharedPtr rclcpp_node, + nav2_util::ParameterEventsSubscriber * param_subscriber) { layered_costmap_ = parent; name_ = name; @@ -57,6 +58,7 @@ 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/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index b39e8a0964..745d745447 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -20,7 +20,6 @@ #include #include "nav2_util/node_thread.hpp" -#include "nav2_util/parameter_events_subscriber.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -126,8 +125,6 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // When creating a local node, this class will launch a separate thread created to spin the node std::unique_ptr rclcpp_thread_; - - std::shared_ptr param_subscriber_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp index e15dd01549..3332940071 100644 --- a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp +++ b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp @@ -30,7 +30,7 @@ struct ParameterEventsCallbackHandle { RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle) - using ParameterEventsCallbackType = std::function; + using ParameterEventsCallbackType = std::function; std::string parameter_name; std::string node_name; @@ -127,7 +127,7 @@ class ParameterEventsSubscriber /// 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. @@ -144,11 +144,11 @@ class ParameterEventsSubscriber /// 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 + * 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. diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 74216894a8..e6efb5f6a9 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -57,8 +57,6 @@ LifecycleNode::LifecycleNode( "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); rclcpp_thread_ = std::make_unique(rclcpp_node_); } - - param_subscriber_ = std::make_shared(shared_from_this()); } LifecycleNode::~LifecycleNode() From 60c0b9a7100edbd1dcfff02ce8f85ab1b6a93dc0 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 19 Nov 2019 16:52:23 -0800 Subject: [PATCH 3/4] use shared ptr in layer --- nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp | 4 ++-- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- nav2_costmap_2d/src/layer.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index c63fd8beea..9819eb38d6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -66,7 +66,7 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface nav2_util::LifecycleNode::SharedPtr node, rclcpp::Node::SharedPtr client_node, rclcpp::Node::SharedPtr rclcpp_node, - nav2_util::ParameterEventsSubscriber * param_subscriber = nullptr); + std::shared_ptr param_subscriber = nullptr); virtual void deactivate() {} /** @brief Stop publishers. */ virtual void activate() {} /** @brief Restart publishers if they've been stopped. */ virtual void reset() {} @@ -137,7 +137,7 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface nav2_util::LifecycleNode::SharedPtr node_; rclcpp::Node::SharedPtr client_node_; rclcpp::Node::SharedPtr rclcpp_node_; - nav2_util::ParameterEventsSubscriber * param_subscriber_; + std::shared_ptr param_subscriber_; std::vector callback_handles_; /** @brief This is called at the end of initialize(). Override to diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 39dbf8c66c..bf0b5ec463 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -148,7 +148,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_.get()); + shared_from_this(), client_node_, rclcpp_node_, param_subscriber_); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 571b4a5b18..c51e43f8db 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -50,7 +50,7 @@ Layer::initialize( nav2_util::LifecycleNode::SharedPtr node, rclcpp::Node::SharedPtr client_node, rclcpp::Node::SharedPtr rclcpp_node, - nav2_util::ParameterEventsSubscriber * param_subscriber) + std::shared_ptr param_subscriber) { layered_costmap_ = parent; name_ = name; From 47244514b15f6be9163f3aa56d583d006e530897 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 19 Nov 2019 17:24:02 -0800 Subject: [PATCH 4/4] add mutex lock for plugin param callbacks --- .../include/nav2_costmap_2d/inflation_layer.hpp | 1 - nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp | 2 ++ nav2_costmap_2d/plugins/inflation_layer.cpp | 2 ++ nav2_costmap_2d/plugins/obstacle_layer.cpp | 4 ++++ nav2_costmap_2d/plugins/voxel_layer.cpp | 9 +++++++++ 5 files changed, 17 insertions(+), 1 deletion(-) 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 090e3c8715..49f21ebe44 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -187,7 +187,6 @@ class InflationLayer : public Layer // Indicates that the entire costmap should be reinflated next time around. bool need_reinflation_; - std::recursive_mutex mutex_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 9819eb38d6..6cd9f3ac3d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -155,6 +155,8 @@ 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/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 931de69539..10988cda84 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -101,12 +101,14 @@ 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; })); diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 5f234a5289..3ab2e7da16 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -252,21 +252,25 @@ 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(); })); } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 1924e336f6..0b3afabf50 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -104,42 +104,51 @@ 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(); })); }