Declare progress checker parameters (#1526)

* Declare progress checker parameters

* Use declare_if_not_declared function

This should allow the node to be brought down and back up using
the lifecycle manager.

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};

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(

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;