Merge pull request #1233 from SteveMacenski/death2magicNumbers

Remove hard-coded numbers from recovery actions

nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp

@@ -28,17 +28,37 @@ namespace nav2_behavior_tree
 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
 public:
-  explicit BackUpAction(const std::string & action_name)
-  : BtActionNode<nav2_msgs::action::BackUp>(action_name)
+  explicit BackUpAction(
+    const std::string & action_name,
+    const BT::NodeParameters & params)
+  : BtActionNode<nav2_msgs::action::BackUp>(action_name, params)
   {
   }
 
   void on_init() override
   {
+    double dist;
+    getParam<double>("backup_dist", dist);
+    double speed;
+    getParam<double>("backup_speed", speed);
+
+    // silently fix, vector direction determined by distance sign
+    if (speed < 0.0) {
+      speed *= -1.0;
+    }
+
     // Populate the input message
-    goal_.target.x = -0.15;
+    goal_.target.x = dist;
     goal_.target.y = 0.0;
     goal_.target.z = 0.0;
+    goal_.speed = speed;
+  }
+
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {
+      {"backup_dist", "-0.15"}, {"backup_speed", "0.025"}};
+    return params;
   }
 };
 

nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp

@@ -31,14 +31,24 @@ namespace nav2_behavior_tree
 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
 public:
-  explicit SpinAction(const std::string & action_name)
-  : BtActionNode<nav2_msgs::action::Spin>(action_name)
+  explicit SpinAction(
+    const std::string & action_name,
+    const BT::NodeParameters & params)
+  : BtActionNode<nav2_msgs::action::Spin>(action_name, params)
   {
   }
 
   void on_init() override
   {
-    goal_.target_yaw = M_PI / 2;
+    double dist;
+    getParam<double>("spin_dist", dist);
+    goal_.target_yaw = dist;
+  }
+
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"spin_dist", "1.57"}};
+    return params;
   }
 };
 

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -23,7 +23,7 @@
       <SequenceStar name="RecoveryActions">
         <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
         <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
-        <Spin/>
+        <Spin spin_dist="1.57"/>
         <Wait wait_duration="5"/>
       </SequenceStar>
     </RecoveryNode>

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -227,7 +227,7 @@ Costmap2DPublisher::costmap_service_callback(
   response->map.metadata.size_x = size_x;
   response->map.metadata.size_y = size_y;
   response->map.metadata.resolution = costmap_->getResolution();
-  response->map.metadata.layer = "Master";
+  response->map.metadata.layer = "master";
   response->map.metadata.map_load_time = current_time;
   response->map.metadata.update_time = current_time;
   response->map.metadata.origin.position.x = costmap_->getOriginX();

nav2_msgs/action/BackUp.action

 #goal definition
 geometry_msgs/Point target
+float32 speed
 ---
 #result definition
 std_msgs/Empty result

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));

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;

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_;
 };
 

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;

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:

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")};

nav2_util/src/costmap.cpp

@@ -87,7 +87,7 @@ void Costmap::set_test_costmap(const TestCostmap & testCostmapType)
 {
   costmap_properties_.map_load_time = node_->now();
   costmap_properties_.update_time = node_->now();
-  costmap_properties_.layer = "Master";
+  costmap_properties_.layer = "master";
   costmap_properties_.resolution = 1;
   costmap_properties_.size_x = 10;
   costmap_properties_.size_y = 10;