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 f34b7632fc7bbb2c29b41f0576307c6eed9b3fc9..5a56395bacaed5777032d808be3fc87f2bcd30d0 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 54681f929fd80ee8151511e555cdbda609abf9b1..a5aa6fc8f728079f8b3a02a74c42bf8db0eb36ae 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 e2fd849b19f5a3a1fb33d5abae8150e670d31ccc..f78dc4e7827bdddd72fcc6a5c7a79fe5ffe82181 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 939cfce668966454cf226cf00f6cc604588e3a43..e8be2e59835a408cc157718511110c232ead3307 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