Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

completely shutdown dyn_params_handler_ (s) #4521

Merged
merged 30 commits into from
Jul 19, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion nav2_amcl/src/amcl_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,8 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
pose_pub_->on_deactivate();
particle_cloud_pub_->on_deactivate();

// reset dynamic parameter handler
// shutdown and reset dynamic parameter handler
remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();

// destroy bond connection
Expand Down
2 changes: 2 additions & 0 deletions nav2_controller/src/controller_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -294,6 +294,8 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)

publishZeroVelocity();
vel_publisher_->on_deactivate();

remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();

// destroy bond connection
Expand Down
4 changes: 4 additions & 0 deletions nav2_costmap_2d/plugins/inflation_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,10 @@ InflationLayer::InflationLayer()

InflationLayer::~InflationLayer()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
delete access_;
}
Expand Down
4 changes: 4 additions & 0 deletions nav2_costmap_2d/plugins/obstacle_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,10 @@ namespace nav2_costmap_2d

ObstacleLayer::~ObstacleLayer()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
for (auto & notifier : observation_notifiers_) {
notifier.reset();
Expand Down
4 changes: 4 additions & 0 deletions nav2_costmap_2d/plugins/static_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,10 @@ StaticLayer::activate()
void
StaticLayer::deactivate()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

Expand Down
4 changes: 4 additions & 0 deletions nav2_costmap_2d/plugins/voxel_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,10 @@ void VoxelLayer::onInitialize()

VoxelLayer::~VoxelLayer()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

Expand Down
1 change: 1 addition & 0 deletions nav2_costmap_2d/src/costmap_2d_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -329,6 +329,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Deactivating");

remove_on_set_parameters_callback(dyn_params_handler.get());
dyn_params_handler.reset();

stop();
Expand Down
1 change: 1 addition & 0 deletions nav2_docking/opennav_docking/src/docking_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,7 @@ DockingServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
navigator_->deactivate();
vel_publisher_->on_deactivate();

remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();
tf2_listener_.reset();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,7 @@ class KinematicsHandler
using Ptr = std::shared_ptr<KinematicsHandler>;

protected:
nav2_util::LifecycleNode::WeakPtr node_;
std::atomic<KinematicParameters *> kinematics_;

// Dynamic parameters handler
Expand Down
6 changes: 6 additions & 0 deletions nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,19 @@ KinematicsHandler::KinematicsHandler()

KinematicsHandler::~KinematicsHandler()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
delete kinematics_.load();
}

void KinematicsHandler::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name)
{
node_ = nh;
plugin_name_ = plugin_name;

declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,13 +70,15 @@ class ParameterHandler
/**
* @brief Destructor for nav2_graceful_controller::ParameterHandler
*/
~ParameterHandler() = default;
~ParameterHandler();

std::mutex & getMutex() {return mutex_;}

Parameters * getParams() {return &params_;}

protected:
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
Expand Down
10 changes: 10 additions & 0 deletions nav2_graceful_controller/src/parameter_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ ParameterHandler::ParameterHandler(
rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name,
rclcpp::Logger & logger, const double costmap_size_x)
{
node_ = node;
plugin_name_ = plugin_name;
logger_ = logger;

Expand Down Expand Up @@ -103,6 +104,15 @@ ParameterHandler::ParameterHandler(
std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
}

ParameterHandler::~ParameterHandler()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

rcl_interfaces::msg::SetParametersResult
ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,11 @@ class ParametersHandler
explicit ParametersHandler(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent);

/**
* @brief Destructor for mppi::ParametersHandler
*/
~ParametersHandler();

/**
* @brief Starts processing dynamic parameter changes
*/
Expand Down
9 changes: 9 additions & 0 deletions nav2_mppi_controller/src/parameters_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,15 @@ ParametersHandler::ParametersHandler(
logger_ = node->get_logger();
}

ParametersHandler::~ParametersHandler()
{
auto node = node_.lock();
if (on_set_param_handler_ && node) {
node->remove_on_set_parameters_callback(on_set_param_handler_.get());
}
on_set_param_handler_.reset();
}

void ParametersHandler::start()
{
auto node = node_.lock();
Expand Down
4 changes: 4 additions & 0 deletions nav2_navfn_planner/src/navfn_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,10 @@ NavfnPlanner::deactivate()
RCLCPP_INFO(
logger_, "Deactivating plugin %s of type NavfnPlanner",
name_.c_str());
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,14 @@ class ParameterHandler
/**
* @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler
*/
~ParameterHandler() = default;
~ParameterHandler();

std::mutex & getMutex() {return mutex_;}

Parameters * getParams() {return &params_;}

protected:
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
Expand Down
10 changes: 10 additions & 0 deletions nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ ParameterHandler::ParameterHandler(
std::string & plugin_name, rclcpp::Logger & logger,
const double costmap_size_x)
{
node_ = node;
plugin_name_ = plugin_name;
logger_ = logger;

Expand Down Expand Up @@ -194,6 +195,15 @@ ParameterHandler::ParameterHandler(
this, std::placeholders::_1));
}

ParameterHandler::~ParameterHandler()
{
auto node = node_.lock();
if (dyn_params_handler_ && node) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

rcl_interfaces::msg::SetParametersResult
ParameterHandler::dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,9 @@ void RotationShimController::deactivate()

primary_controller_->deactivate();

if (auto node = node_.lock()) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ void AStarAlgorithm<NodeT>::initialize(
_max_on_approach_iterations = max_on_approach_iterations;
_terminal_checking_interval = terminal_checking_interval;
_max_planning_time = max_planning_time;
if(!_is_initialized) {
if (!_is_initialized) {
NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
}
_is_initialized = true;
Expand Down
5 changes: 5 additions & 0 deletions nav2_smac_planner/src/smac_planner_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,6 +177,11 @@ void SmacPlanner2D::deactivate()
if (_costmap_downsampler) {
_costmap_downsampler->on_deactivate();
}
// shutdown dyn_param_handler
auto node = _node.lock();
if (_dyn_params_handler && node) {
node->remove_on_set_parameters_callback(_dyn_params_handler.get());
}
_dyn_params_handler.reset();
}

Expand Down
5 changes: 5 additions & 0 deletions nav2_smac_planner/src/smac_planner_hybrid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -311,6 +311,11 @@ void SmacPlannerHybrid::deactivate()
if (_costmap_downsampler) {
_costmap_downsampler->on_deactivate();
}
// shutdown dyn_param_handler
auto node = _node.lock();
if (_dyn_params_handler && node) {
node->remove_on_set_parameters_callback(_dyn_params_handler.get());
}
_dyn_params_handler.reset();
}

Expand Down
5 changes: 5 additions & 0 deletions nav2_smac_planner/src/smac_planner_lattice.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -250,6 +250,11 @@ void SmacPlannerLattice::deactivate()
_expansions_publisher->on_deactivate();
_planned_footprints_publisher->on_deactivate();
}
// shutdown dyn_param_handler
auto node = _node.lock();
if (_dyn_params_handler && node) {
node->remove_on_set_parameters_callback(_dyn_params_handler.get());
}
_dyn_params_handler.reset();
}

Expand Down
5 changes: 5 additions & 0 deletions nav2_theta_star_planner/src/theta_star_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,11 @@ void ThetaStarPlanner::activate()
void ThetaStarPlanner::deactivate()
{
RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str());
auto node = parent_node_.lock();
if (node && dyn_params_handler_) {
node->remove_on_set_parameters_callback(dyn_params_handler_.get());
}
dyn_params_handler_.reset();
}

nav_msgs::msg::Path ThetaStarPlanner::createPlan(
Expand Down
2 changes: 2 additions & 0 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,8 @@ VelocitySmoother::on_deactivate(const rclcpp_lifecycle::State &)
timer_.reset();
}
smoothed_cmd_pub_->on_deactivate();

remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();

// destroy bond connection
Expand Down
1 change: 1 addition & 0 deletions nav2_waypoint_follower/src/waypoint_follower.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -154,6 +154,7 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/)

xyz_action_server_->deactivate();
gps_action_server_->deactivate();
remove_on_set_parameters_callback(dyn_params_handler_.get());
dyn_params_handler_.reset();
// destroy bond connection
destroyBond();
Expand Down
Loading