diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/progress_checker.hpp index fc87316e3f5112efb2e796b4bf7ce365e55a0492..3d754ad7aa2fdb0eb4c50d9556180915b97cf95f 100644 --- a/nav2_controller/include/nav2_controller/progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/progress_checker.hpp @@ -16,6 +16,7 @@ #define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" @@ -33,7 +34,7 @@ public: * @brief Constructor of ProgressChecker * @param node Node pointer */ - explicit ProgressChecker(const rclcpp::Node::SharedPtr & node); + explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node); /** * @brief Checks if the robot has moved compare to previous * pose @@ -59,7 +60,7 @@ protected: */ void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); - rclcpp::Node::SharedPtr nh_; + rclcpp_lifecycle::LifecycleNode::SharedPtr nh_; double radius_; rclcpp::Duration time_allowance_{0, 0}; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index c0c8926cfc7faf72f05ddef848696ef0f511a6eb..16d09e2b28b8400bce6f94a2b198c5d55faa63a1 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -68,7 +68,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); - progress_checker_ = std::make_unique<ProgressChecker>(rclcpp_node_); + progress_checker_ = std::make_unique<ProgressChecker>(node); if (controller_types_.size() != controller_ids_.size()) { RCLCPP_FATAL( diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/src/progress_checker.cpp index 7f4df3bbda2e26f14398692fed2adc93b7dd4add..def27aaca521830ee4918fc3a2d1eb8d681fab1d 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/src/progress_checker.cpp @@ -18,14 +18,19 @@ #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" namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); -ProgressChecker::ProgressChecker(const rclcpp::Node::SharedPtr & node) +ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) : nh_(node) { + nav2_util::declare_parameter_if_not_declared( + nh_, "required_movement_radius", rclcpp::ParameterValue(0.5)); + nav2_util::declare_parameter_if_not_declared( + nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 nh_->get_parameter_or("required_movement_radius", radius_, 0.5); double time_allowance_param;