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 0ca175d47b..9a6f195e39 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()