diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index b66ad9b2ee15dfba8802dcd2000f0d4811d78ca3..a35b8d46565b63e6c8fcd08b483d11c89b9c8eb4 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -17,6 +17,7 @@ #include <memory> #include "back_up.hpp" +#include "nav2_util/node_utils.hpp" using namespace std::chrono_literals; @@ -26,7 +27,6 @@ namespace nav2_recoveries BackUp::BackUp() : Recovery<BackUpAction>() { - simulate_ahead_time_ = 2.0; } BackUp::~BackUp() @@ -35,8 +35,9 @@ BackUp::~BackUp() void BackUp::onConfigure() { - node_->declare_parameter("simulate_ahead_time", simulate_ahead_time_); - simulate_ahead_time_ = node_->get_parameter("simulate_ahead_time").as_double(); + nav2_util::declare_parameter_if_not_declared(node_, + "simulate_ahead_time", rclcpp::ParameterValue(2.0)); + node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); } Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index c86c888316713b51a0ecfee1dc50289358c94500..66693f5495752abcad9b56d921a3d77f3281734b 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -25,6 +25,21 @@ RecoveryServer::RecoveryServer() : nav2_util::LifecycleNode("nav2_recoveries", "", true), plugin_loader_("nav2_core", "nav2_core::Recovery") { + declare_parameter("costmap_topic", + rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); + declare_parameter("footprint_topic", + rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); + + std::vector<std::string> plugin_names{std::string("Spin"), + std::string("BackUp"), std::string("Wait")}; + std::vector<std::string> plugin_types{std::string("nav2_recoveries/Spin"), + std::string("nav2_recoveries/BackUp"), + std::string("nav2_recoveries/Wait")}; + + declare_parameter("plugin_names", + rclcpp::ParameterValue(plugin_names)); + declare_parameter("plugin_types", + rclcpp::ParameterValue(plugin_types)); } @@ -44,24 +59,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_->setCreateTimerInterface(timer_interface); transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_); - declare_parameter("costmap_topic", - rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); - declare_parameter("footprint_topic", - rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); - - std::vector<std::string> plugin_names{std::string("Spin"), - std::string("BackUp"), std::string("Wait")}; - std::vector<std::string> plugin_types{std::string("nav2_recoveries/Spin"), - std::string("nav2_recoveries/BackUp"), - std::string("nav2_recoveries/Wait")}; - - declare_parameter("plugin_names", - rclcpp::ParameterValue(plugin_names)); - declare_parameter("plugin_types", - rclcpp::ParameterValue(plugin_types)); - - plugin_names_ = get_parameter("plugin_names").as_string_array(); - plugin_types_ = get_parameter("plugin_types").as_string_array(); + get_parameter("plugin_names", plugin_names_); + get_parameter("plugin_types", plugin_types_); loadRecoveryPlugins();