From cc28e8a27aad8be919d08d6cb3ee46f7d618a935 Mon Sep 17 00:00:00 2001
From: Carl Delsey <carl.r.delsey@intel.com>
Date: Fri, 14 Feb 2020 16:36:28 -0800
Subject: [PATCH] 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.
---
.../include/nav2_controller/progress_checker.hpp | 5 +++--
nav2_controller/src/nav2_controller.cpp | 2 +-
nav2_controller/src/progress_checker.cpp | 7 ++++++-
3 files changed, 10 insertions(+), 4 deletions(-)
diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/progress_checker.hpp
index fc87316e..3d754ad7 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 c0c8926c..16d09e2b 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 7f4df3bb..def27aac 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;
--
GitLab