diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 48eb580c964..7d5f93febec 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_controller/plugins/simple_progress_checker.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -31,6 +32,41 @@ namespace nav2_controller class PoseProgressChecker : public SimpleProgressChecker { + struct Parameters + { + double required_movement_angle; + }; + +/** + * @class nav2_controller::PoseProgressChecker::ParameterHandler + * @brief This class handls parameters and dynamic parameter updates for the nav2_controller. + */ + class ParameterHandler + { +public: + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger); + ~ParameterHandler(); + std::shared_ptr getParams() {return std::make_shared(params_);} + +protected: + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + void + updateParametersCallback( + std::vector parameters); + + rcl_interfaces::msg::SetParametersResult + validateParameterUpdatesCallback( + std::vector parameters); + rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; + }; + public: void initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -50,18 +86,14 @@ class PoseProgressChecker : public SimpleProgressChecker const geometry_msgs::msg::Pose2D &); double required_movement_angle_; - - // Dynamic parameters handler - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::shared_ptr params_; std::string plugin_name_; - /** - * @brief Callback executed when a parameter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); + rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")}; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::unique_ptr param_handler_; }; + } // namespace nav2_controller #endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_ diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 271e5635c07..b9d1a316f00 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -12,17 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/plugins/pose_progress_checker.hpp" #include #include #include #include + #include "angles/angles.h" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" #include "pluginlib/class_list_macros.hpp" +#include "nav2_controller/plugins/pose_progress_checker.hpp" +#include "nav2_core/controller_exceptions.hpp" using rcl_interfaces::msg::ParameterType; using std::placeholders::_1; @@ -30,21 +32,95 @@ using std::placeholders::_1; namespace nav2_controller { +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +PoseProgressChecker::ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger) +{ + node_ = node; + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared(node, plugin_name + ".required_movement_angle", + rclcpp::ParameterValue(0.5)); + node->get_parameter(plugin_name + ".required_movement_angle", + params_.required_movement_angle); + on_set_params_handler_ = node->add_on_set_parameters_callback( + [this](const auto & params) { + return this->validateParameterUpdatesCallback(params); + }); + post_set_params_handler_ = node->add_post_set_parameters_callback( + [this](const auto & params) { + return this->updateParametersCallback(params); + }); +} +PoseProgressChecker::ParameterHandler::~ParameterHandler() +{ + auto node = node_.lock(); + if (post_set_params_handler_ && node) { + node->remove_post_set_parameters_callback(post_set_params_handler_.get()); + } + post_set_params_handler_.reset(); + if (on_set_params_handler_ && node) { + node->remove_on_set_parameters_callback(on_set_params_handler_.get()); + } + on_set_params_handler_.reset(); +} +rcl_interfaces::msg::SetParametersResult +PoseProgressChecker::ParameterHandler::validateParameterUpdatesCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + { + if (name.find(plugin_name_ + ".") != 0) { + continue; + } + if (name == plugin_name_ + ".required_movement_angle" && + type == ParameterType::PARAMETER_DOUBLE && + (parameter.as_double() <= 0.0 || parameter.as_double() >= 2 * M_PI)) + { + result.successful = false; + result.reason = "The value required_movement_angle is incorrectly set, " + "it should be 0 < required_movement_angle < 2PI. Ignoring parameter update."; + return result; + } + } + } + result.successful = true; + return result; +} +void PoseProgressChecker::ParameterHandler::updateParametersCallback( + std::vector parameters) +{ + for (const auto & param : parameters) { + const auto & name = param.get_name(); + const auto & type = param.get_type(); + if (name == plugin_name_ + ".required_movement_angle" && + type == ParameterType::PARAMETER_DOUBLE) + { + params_.required_movement_angle = param.as_double(); + } + } +} void PoseProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + auto node = parent.lock(); + if (!node) { + throw nav2_core::ControllerException("Unable to lock node!"); + } + node_ = parent; plugin_name_ = plugin_name; + logger_ = node->get_logger(); + param_handler_ = std::make_unique(node, plugin_name_, logger_); + params_ = param_handler_->getParams(); SimpleProgressChecker::initialize(parent, plugin_name); - auto node = parent.lock(); - - nav2_util::declare_parameter_if_not_declared( - node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5)); - node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5); - - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1)); } bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) @@ -64,7 +140,7 @@ bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose) { return pose_distance(pose, baseline_pose_) > radius_ || - poseAngleDistance(pose, baseline_pose_) > required_movement_angle_; + poseAngleDistance(pose, baseline_pose_) > params_->required_movement_angle; } double PoseProgressChecker::poseAngleDistance( @@ -74,23 +150,6 @@ double PoseProgressChecker::poseAngleDistance( return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta)); } -rcl_interfaces::msg::SetParametersResult -PoseProgressChecker::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_angle") { - required_movement_angle_ = parameter.as_double(); - } - } - } - result.successful = true; - return result; -} } // namespace nav2_controller