Skip to content
Snippets Groups Projects
Commit 94a8d373 authored by bpwilcox's avatar bpwilcox
Browse files

ROS1 parity dynamic parameter callbacks for costmap_2d

add lambda callbacks directly
parent 56a09275
No related branches found
No related tags found
No related merge requests found
Showing
with 221 additions and 14 deletions
......@@ -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 @@ protected:
// 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 @@ protected:
std::vector<geometry_msgs::msg::Point> padded_footprint_;
std::unique_ptr<ClearCostmapService> clear_costmap_service_;
std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
};
} // namespace nav2_costmap_2d
......
......@@ -169,6 +169,8 @@ private:
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 @@ private:
// Indicates that the entire costmap should be reinflated next time around.
bool need_reinflation_;
std::recursive_mutex mutex_;
};
} // namespace nav2_costmap_2d
......
......@@ -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 @@ public:
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 @@ protected:
nav2_util::LifecycleNode::SharedPtr node_;
rclcpp::Node::SharedPtr client_node_;
rclcpp::Node::SharedPtr rclcpp_node_;
nav2_util::ParameterEventsSubscriber * param_subscriber_;
std::vector<nav2_util::ParameterEventsCallbackHandle::SharedPtr> callback_handles_;
/** @brief This is called at the end of initialize(). Override to
* implement subclass-specific initialization.
......
......@@ -160,6 +160,8 @@ protected:
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
......
......@@ -85,7 +85,7 @@ protected:
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,
......
......@@ -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<bool>();
need_reinflation_ = true;
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".inflate_unknown",
[&](const rclcpp::Parameter & p) {
inflate_unknown_ = p.get_value<bool>();
need_reinflation_ = true;
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".cost_scaling_factor",
[&](const rclcpp::Parameter & p) {
std::lock_guard<std::recursive_mutex> lock(mutex_);
cost_scaling_factor_ = p.get_value<double>();
need_reinflation_ = true;
computeCaches();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".inflation_radius",
[&](const rclcpp::Parameter & p) {
std::lock_guard<std::recursive_mutex> lock(mutex_);
inflation_radius_ = p.get_value<double>();
cell_inflation_radius_ = cellDistance(inflation_radius_);
need_reinflation_ = true;
computeCaches();
}));
}
}
void
......
......@@ -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<bool>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".footprint_clearing_enabled",
[&](const rclcpp::Parameter & p) {
footprint_clearing_enabled_ = p.get_value<bool>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".max_obstacle_height",
[&](const rclcpp::Parameter & p) {
max_obstacle_height_ = p.get_value<double>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".combination_method",
[&](const rclcpp::Parameter & p) {
combination_method_ = p.get_value<int>();
}));
}
}
void
......
......@@ -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<bool>()) {
enabled_ = p.get_value<bool>();
has_updated_data_ = true;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
}
};
if (param_subscriber_) {
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled", cb));
}
}
void
......
......@@ -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<bool>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".footprint_clearing_enabled",
[&](const rclcpp::Parameter & p) {
footprint_clearing_enabled_ = p.get_value<bool>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".max_obstacle_height",
[&](const rclcpp::Parameter & p) {
max_obstacle_height_ = p.get_value<double>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".combination_method",
[&](const rclcpp::Parameter & p) {
combination_method_ = p.get_value<int>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels",
[&](const rclcpp::Parameter & p) {
size_z_ = p.get_value<int>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z",
[&](const rclcpp::Parameter & p) {
origin_z_ = p.get_value<double>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution",
[&](const rclcpp::Parameter & p) {
z_resolution_ = p.get_value<double>();
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
".unknown_threshold",
[&](const rclcpp::Parameter & p) {
unknown_threshold_ = p.get_value<int>() + (VOXEL_BITS - size_z_);
}));
callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold",
[&](const rclcpp::Parameter & p) {
mark_threshold_ = p.get_value<int>();
}));
}
}
VoxelLayer::~VoxelLayer()
......
......@@ -137,6 +137,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
tf_buffer_->setCreateTimerInterface(timer_interface);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(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<ClearCostmapService>(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<geometry_msgs::msg::Point> 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<geometry_msgs::msg::Point> 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()
{
......
......@@ -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();
}
......
......@@ -20,7 +20,6 @@
#include <thread>
#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 @@ protected:
// When creating a local node, this class will launch a separate thread created to spin the node
std::unique_ptr<NodeThread> rclcpp_thread_;
std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
};
} // namespace nav2_util
......
......@@ -30,7 +30,7 @@ struct ParameterEventsCallbackHandle
{
RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle)
using ParameterEventsCallbackType = std::function<void(const rclcpp::Parameter &)>;
using ParameterEventsCallbackType = std::function<void (const rclcpp::Parameter &)>;
std::string parameter_name;
std::string node_name;
......@@ -127,7 +127,7 @@ public:
/// 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 @@ public:
/// 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.
......
......@@ -57,8 +57,6 @@ LifecycleNode::LifecycleNode(
"_", namespace_, rclcpp::NodeOptions(options).arguments(new_args));
rclcpp_thread_ = std::make_unique<NodeThread>(rclcpp_node_);
}
param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(shared_from_this());
}
LifecycleNode::~LifecycleNode()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment