remove magic numbers from BT

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.59"}};
+    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.59"/>
         <Wait wait_duration="5"/>
       </SequenceStar>
     </RecoveryNode>

nav2_msgs/action/BackUp.action

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