From 3525e0ce4d94afe71e73e73ddf4436aac1a28171 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Fri, 11 Oct 2019 17:14:53 -0700
Subject: [PATCH] remove magic numbers from BT
---
.../nav2_behavior_tree/back_up_action.hpp | 26 ++++++++++++++++---
.../nav2_behavior_tree/spin_action.hpp | 16 +++++++++---
.../navigate_w_replanning_and_recovery.xml | 2 +-
nav2_msgs/action/BackUp.action | 1 +
4 files changed, 38 insertions(+), 7 deletions(-)
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
index f34b7632..5a56395b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
+++ b/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;
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
index 54681f92..a5aa6fc8 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
+++ b/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;
}
};
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index e2fd849b..f78dc4e7 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/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>
diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action
index 939cfce6..e8be2e59 100644
--- a/nav2_msgs/action/BackUp.action
+++ b/nav2_msgs/action/BackUp.action
@@ -1,5 +1,6 @@
#goal definition
geometry_msgs/Point target
+float32 speed
---
#result definition
std_msgs/Empty result
--
GitLab