In wait_action node change duration type from int to double (ros-navi…

…gation#3871) (#30) * change duration tyype from int to double * change "1" to "1.0" as default wait_duration value * modify wait_action tests * fix * change int values to double in trees * add usage of Duration::from_seconds() * fix build failing issue * delete whitespace Co-authored-by: maksymdidukh <[email protected]>
EnjoyRobotics · Oct 30, 2024 · 756a9eb · 756a9eb
1 parent c3ca086
commit 756a9eb
Show file tree

Hide file tree

Showing 8 changed files with 15 additions and 14 deletions.
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
@@ -53,7 +53,7 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
   {
     return providedBasicPorts(
       {
-        BT::InputPort<int>("wait_duration", 1, "Wait time")
+        BT::InputPort<double>("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
@@ -14,6 +14,7 @@
 
 #include <string>
 #include <memory>
+#include <cmath>
 
 #include "nav2_behavior_tree/plugins/action/wait_action.hpp"
 
@@ -26,16 +27,16 @@ WaitAction::WaitAction(
   const BT::NodeConfiguration & conf)
 : BtActionNode<nav2_msgs::action::Wait>(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
@@ -120,18 +120,18 @@ TEST_F(WaitActionTestFixture, test_ports)
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 1);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("wait_duration"), 1.0);
 
   xml_txt =
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <Wait wait_duration="10" />
+            <Wait wait_duration="10.0" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  EXPECT_EQ(tree_->rootNode()->getInput<int>("wait_duration"), 10);
+  EXPECT_EQ(tree_->rootNode()->getInput<double>("wait_duration"), 10.0);
 }
 
 TEST_F(WaitActionTestFixture, test_tick)
@@ -140,7 +140,7 @@ TEST_F(WaitActionTestFixture, test_tick)
     R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <Wait wait_duration="-5"/>
+            <Wait wait_duration="-5.0"/>
         </BehaviorTree>
       </root>)";
 

diff --git a/...tor/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/...tor/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml
@@ -37,7 +37,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>

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
@@ -29,7 +29,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}"/>
         </RoundRobin>
       </ReactiveFallback>

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
@@ -38,7 +38,7 @@
               <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
             </Sequence>
             <Spin spin_dist="1.57" error_code_id="{spin_error_code}"/>
-            <Wait wait_duration="5"/>
+            <Wait wait_duration="5.0"/>
             <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_code_id}"/>
           </RoundRobin>
         </ReactiveFallback>

diff --git a/..._bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/..._bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml
@@ -20,7 +20,7 @@
             <RetryUntilSuccessful num_attempts="1">
               <SequenceStar name="CancelingControlAndWait">
                 <CancelControl name="ControlCancel"/>
-                <Wait wait_duration="5"/>
+                <Wait wait_duration="5.0"/>
               </SequenceStar>
             </RetryUntilSuccessful>
           </PathLongerOnApproach>
@@ -38,7 +38,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin spin_dist="1.57"/>
-          <Wait wait_duration="5"/>
+          <Wait wait_duration="5.0"/>
           <BackUp backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>

diff --git a/...igator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/...igator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml
@@ -35,7 +35,7 @@
             <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
           </Sequence>
           <Spin name="SpinRecovery" spin_dist="1.57"/>
-          <Wait name="WaitRecovery" wait_duration="5"/>
+          <Wait name="WaitRecovery" wait_duration="5.0"/>
           <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.05"/>
         </RoundRobin>
       </ReactiveFallback>
-Original file line number
+Diff line change
@@ Expand Up @@
       {
         return providedBasicPorts(
           {
-            BT::InputPort<int>("wait_duration", 1, "Wait time")
+            BT::InputPort<double>("wait_duration", 1.0, "Wait time")
           });
       }
     };
@@ Expand Down @@