diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index f452d24d320..5143f11351e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -53,7 +53,7 @@ class WaitAction : public BtActionNode { return providedBasicPorts( { - BT::InputPort("wait_duration", 1, "Wait time") + BT::InputPort("wait_duration", 1.0, "Wait time") }); } }; diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index d2b0e6484be..f08fed31476 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "nav2_behavior_tree/plugins/action/wait_action.hpp" @@ -26,16 +27,16 @@ WaitAction::WaitAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { - int duration; + double duration; getInput("wait_duration", duration); if (duration <= 0) { RCLCPP_WARN( node_->get_logger(), "Wait duration is negative or zero " - "(%i). Setting to positive.", duration); + "(%f). Setting to positive.", duration); duration *= -1; } - goal_.time.sec = duration; + goal_.time = rclcpp::Duration::from_seconds(duration); } void WaitAction::on_tick() diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 807c89ed0c9..774622711b2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -120,18 +120,18 @@ TEST_F(WaitActionTestFixture, test_ports) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 1); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 1.0); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 10); + EXPECT_EQ(tree_->rootNode()->getInput("wait_duration"), 10.0); } TEST_F(WaitActionTestFixture, test_tick) @@ -140,7 +140,7 @@ TEST_F(WaitActionTestFixture, test_tick) R"( - + )"; diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 0a00a60a4f5..b1d470435ef 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -37,7 +37,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index c5056c1c199..9f5c6f51d58 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -29,7 +29,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 6843f3d8592..228c7ad223d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -38,7 +38,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index e2bbeb12c73..9de1f28e910 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -20,7 +20,7 @@ - + @@ -38,7 +38,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index bd4e1c9aca8..aac9a72ae0e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -35,7 +35,7 @@ - +