From 07cb037e7cd5a3f7d1112df204853097fd6a875b Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 11 Oct 2019 17:15:30 -0700
Subject: [PATCH] remove magic numbers from recoveries
---
.../include/nav2_recoveries/recovery.hpp | 3 ++-
nav2_recoveries/plugins/back_up.cpp | 3 ++-
nav2_recoveries/plugins/back_up.hpp | 1 +
nav2_recoveries/plugins/spin.cpp | 25 +++++++++++++++----
nav2_recoveries/plugins/spin.hpp | 2 +-
nav2_recoveries/src/recovery_server.cpp | 1 +
6 files changed, 27 insertions(+), 8 deletions(-)
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index 808629a6..6397822f 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 a35b8d46..9424dcd3 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 bef51343..3ce3623e 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 931376a6..a76c1ba0 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 a24d5cb3..358fdec1 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 d2dc9966..f6f6aa28 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")};
--
GitLab