diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 808629a65ca77155343fa5a7ffa415f8b765618e..6397822ffd13369281e1cee834087cf9536dbf05 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -51,7 +51,7 @@ public: Recovery() : action_server_(nullptr), - cycle_frequency_(10), + cycle_frequency_(10.0), enabled_(false) { } @@ -100,6 +100,7 @@ public: node_->get_parameter("costmap_topic", costmap_topic); node_->get_parameter("footprint_topic", footprint_topic); + node_->get_parameter("cycle_frequency", cycle_frequency_); action_server_ = std::make_unique<ActionServer>(node_, recovery_name_, std::bind(&Recovery::execute, this)); diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index a35b8d46565b63e6c8fcd08b483d11c89b9c8eb4..9424dcd396887a40da6912b9adb3c73355e7b22b 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -48,6 +48,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command) } command_x_ = command->target.x; + command_speed_ = command->speed; if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); @@ -77,7 +78,7 @@ Status BackUp::onCycleUpdate() geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.y = 0.0; cmd_vel.angular.z = 0.0; - command_x_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025; + command_x_ < 0 ? cmd_vel.linear.x = -command_speed_ : cmd_vel.linear.x = command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index bef5134304957ed92d41f98cb92c220076a41d35..3ce3623e5a4a65c0e4234a866c8ba68f36cac106 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -49,6 +49,7 @@ protected: geometry_msgs::msg::PoseStamped initial_pose_; double command_x_; + double command_speed_; double simulate_ahead_time_; }; diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 931376a6fa8cd8cfeec1748a18df946d58d720ad..a76c1ba024fdee5c15362222d5581bec7005e9a8 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -26,6 +26,7 @@ #pragma GCC diagnostic pop #include "tf2/LinearMath/Quaternion.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "nav2_util/node_utils.hpp" using namespace std::chrono_literals; @@ -35,18 +36,32 @@ namespace nav2_recoveries Spin::Spin() : Recovery<SpinAction>() { - // TODO(orduno) #378 Pull values from the robot - max_rotational_vel_ = 1.0; - min_rotational_vel_ = 0.4; - rotational_acc_lim_ = 3.2; initial_yaw_ = 0.0; - simulate_ahead_time_ = 2.0; } Spin::~Spin() { } +void Spin::onConfigure() +{ + nav2_util::declare_parameter_if_not_declared(node_, + "simulate_ahead_time", rclcpp::ParameterValue(2.0)); + node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); + + nav2_util::declare_parameter_if_not_declared(node_, + "max_rotational_vel", rclcpp::ParameterValue(1.0)); + node_->get_parameter("max_rotational_vel", max_rotational_vel_); + + nav2_util::declare_parameter_if_not_declared(node_, + "min_rotational_vel", rclcpp::ParameterValue(0.4)); + node_->get_parameter("min_rotational_vel", min_rotational_vel_); + + nav2_util::declare_parameter_if_not_declared(node_, + "rotational_acc_lim", rclcpp::ParameterValue(3.2)); + node_->get_parameter("rotational_acc_lim", rotational_acc_lim_); +} + Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command) { geometry_msgs::msg::PoseStamped current_pose; diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index a24d5cb3b495af3eb1d016083b3cf19d37c03c8e..358fdec1c9c4c61c1c85d2f21eec3ee4d6e6dd8a 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -34,7 +34,7 @@ public: ~Spin(); Status onRun(const std::shared_ptr<const SpinAction::Goal> command) override; - + void onConfigure() override; Status onCycleUpdate() override; protected: diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index d2dc99660b23231f98dc000f1b26832f445d57d5..f6f6aa28c146aeac4313bbdbefd2be663ab95dc8 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -29,6 +29,7 @@ RecoveryServer::RecoveryServer() rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); declare_parameter("footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); + declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0)); std::vector<std::string> plugin_names{std::string("Spin"), std::string("BackUp"), std::string("Wait")};