diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index b8e976a02f7..763ff3ae9a8 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -39,7 +39,7 @@ #include #include #include - +#include #include "nav2_ros_common/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -145,7 +145,14 @@ class KinematicsHandler void activate(); void deactivate(); - inline KinematicParameters getKinematics() {return *kinematics_.load();} + inline KinematicParameters getKinematics() + { + KinematicParameters * ptr = kinematics_.load(); + if (ptr == nullptr) { + throw std::runtime_error("Can't call KinematicsHandler::getKinematics()."); + } + return *ptr; + } void setSpeedLimit(const double & speed_limit, const bool & percentage); diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index b6512272e86..e51778b7189 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -33,7 +33,7 @@ */ #include "dwb_plugins/kinematic_parameters.hpp" - +#include #include #include #include @@ -55,7 +55,10 @@ KinematicsHandler::KinematicsHandler() KinematicsHandler::~KinematicsHandler() { - delete kinematics_.load(); + KinematicParameters * ptr = kinematics_.load(); + if (ptr != nullptr) { + delete ptr; + } } void KinematicsHandler::initialize( @@ -151,7 +154,11 @@ void KinematicsHandler::deactivate() void KinematicsHandler::setSpeedLimit( const double & speed_limit, const bool & percentage) { - KinematicParameters kinematics(*kinematics_.load()); + KinematicParameters * ptr = kinematics_.load(); + if (ptr == nullptr) { + return; // Nothing to update + } + KinematicParameters kinematics(*ptr); if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value @@ -208,9 +215,9 @@ rcl_interfaces::msg::SetParametersResult KinematicsHandler::validateParameterUpd param_name == plugin_name_ + ".acc_lim_theta")) { RCLCPP_WARN( - logger_, "The value of parameter '%s' is incorrectly set to %f, " - "it should be >= 0. Ignoring parameter update.", - param_name.c_str(), parameter.as_double()); + logger_, "The value of parameter '%s' is incorrectly set to %f, " + "it should be >= 0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); result.successful = false; } else if (parameter.as_double() > 0.0 && // NOLINT (param_name == plugin_name_ + ".decel_lim_x" || @@ -218,9 +225,9 @@ rcl_interfaces::msg::SetParametersResult KinematicsHandler::validateParameterUpd param_name == plugin_name_ + ".decel_lim_theta")) { RCLCPP_WARN( - logger_, "The value of parameter '%s' is incorrectly set to %f, " - "it should be <= 0. Ignoring parameter update.", - param_name.c_str(), parameter.as_double()); + logger_, "The value of parameter '%s' is incorrectly set to %f, " + "it should be <= 0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); result.successful = false; } } @@ -232,7 +239,11 @@ void KinematicsHandler::updateParametersCallback(std::vector parameters) { rcl_interfaces::msg::SetParametersResult result; - KinematicParameters kinematics(*kinematics_.load()); + KinematicParameters * ptr = kinematics_.load(); + if (ptr == nullptr) { + return; // Nothing to update + } + KinematicParameters kinematics(*ptr); for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); @@ -284,8 +295,11 @@ KinematicsHandler::updateParametersCallback(std::vector param void KinematicsHandler::update_kinematics(KinematicParameters kinematics) { - delete kinematics_.load(); - kinematics_.store(new KinematicParameters(kinematics)); + KinematicParameters * new_kinematics = new KinematicParameters(kinematics); + KinematicParameters * old_kinematics = kinematics_.exchange(new_kinematics); + if (old_kinematics != nullptr) { + delete old_kinematics; + } } } // namespace dwb_plugins