Skip to content
Snippets Groups Projects
Unverified Commit cc28e8a2 authored by Carl Delsey's avatar Carl Delsey Committed by GitHub
Browse files

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.
parent 1928e491
No related branches found
No related tags found
No related merge requests found
...@@ -16,6 +16,7 @@ ...@@ -16,6 +16,7 @@
#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_ #define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp" #include "geometry_msgs/msg/pose2_d.hpp"
...@@ -33,7 +34,7 @@ public: ...@@ -33,7 +34,7 @@ public:
* @brief Constructor of ProgressChecker * @brief Constructor of ProgressChecker
* @param node Node pointer * @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 * @brief Checks if the robot has moved compare to previous
* pose * pose
...@@ -59,7 +60,7 @@ protected: ...@@ -59,7 +60,7 @@ protected:
*/ */
void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose);
rclcpp::Node::SharedPtr nh_; rclcpp_lifecycle::LifecycleNode::SharedPtr nh_;
double radius_; double radius_;
rclcpp::Duration time_allowance_{0, 0}; rclcpp::Duration time_allowance_{0, 0};
......
...@@ -68,7 +68,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) ...@@ -68,7 +68,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
auto node = shared_from_this(); 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()) { if (controller_types_.size() != controller_ids_.size()) {
RCLCPP_FATAL( RCLCPP_FATAL(
......
...@@ -18,14 +18,19 @@ ...@@ -18,14 +18,19 @@
#include "nav_2d_utils/conversions.hpp" #include "nav_2d_utils/conversions.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp" #include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_controller namespace nav2_controller
{ {
static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); 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) : 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 // 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); nh_->get_parameter_or("required_movement_radius", radius_, 0.5);
double time_allowance_param; double time_allowance_param;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment