Address reviewer's comments

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

nav2_behavior_tree/plugins/decorator/distance_controller.hpp → nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp

@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
-#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_
 
 #include <memory>
 #include <string>
@@ -63,4 +63,4 @@ class DistanceController : public BT::DecoratorNode
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_

nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp → nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp

@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
-#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_
 
 #include <string>
 #include <memory>
@@ -55,10 +55,11 @@ class DistanceTraveledCondition : public BT::ConditionNode
   geometry_msgs::msg::PoseStamped start_pose_;
 
   double distance_;
+  double transform_tolerance_;
   std::string global_frame_;
   std::string robot_base_frame_;
 };
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/time_expired_condition.hpp → nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp

@@ -13,12 +13,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
-#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_
 
-#include <chrono>
 #include <string>
 
+#include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp_v3/condition_node.h"
 
 namespace nav2_behavior_tree
@@ -44,10 +44,11 @@ class TimeExpiredCondition : public BT::ConditionNode
   }
 
 private:
-  std::chrono::time_point<std::chrono::high_resolution_clock> start_;
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Time start_;
   double period_;
 };
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -22,7 +22,7 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 
-#include "distance_traveled_condition.hpp"
+#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -38,20 +38,27 @@ DistanceTraveledCondition::DistanceTraveledCondition(
   getInput("robot_base_frame", robot_base_frame_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
 }
 
 BT::NodeStatus DistanceTraveledCondition::tick()
 {
   if (status() == BT::NodeStatus::IDLE) {
-    if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
+    if (!nav2_util::getCurrentPose(
+        start_pose_, *tf_, global_frame_, robot_base_frame_,
+        transform_tolerance_))
+    {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
     }
     return BT::NodeStatus::FAILURE;
   }
 
   // Determine distance travelled since we've started this iteration
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, global_frame_, robot_base_frame_)) {
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
     RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
     return BT::NodeStatus::FAILURE;
   }

nav2_behavior_tree/plugins/condition/time_expired_condition.cpp

@@ -16,13 +16,12 @@
 #ifndef NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
 #define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
 
-#include <chrono>
 #include <string>
 #include <memory>
 
 #include "behaviortree_cpp_v3/condition_node.h"
 
-#include "time_expired_condition.hpp"
+#include "nav2_behavior_tree/plugins/time_expired_condition.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -34,28 +33,28 @@ TimeExpiredCondition::TimeExpiredCondition(
   period_(1.0)
 {
   getInput("seconds", period_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  start_ = node_->now();
 }
 
 BT::NodeStatus TimeExpiredCondition::tick()
 {
   if (status() == BT::NodeStatus::IDLE) {
-    start_ = std::chrono::high_resolution_clock::now();
+    start_ = node_->now();
     return BT::NodeStatus::FAILURE;
   }
 
   // Determine how long its been since we've started this iteration
-  auto now = std::chrono::high_resolution_clock::now();
-  auto elapsed = now - start_;
+  auto elapsed = node_->now() - start_;
 
   // Now, get that in seconds
-  typedef std::chrono::duration<float> float_seconds;
-  auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
+  auto seconds = elapsed.seconds();
 
-  if (seconds.count() < period_) {
+  if (seconds < period_) {
     return BT::NodeStatus::FAILURE;
   }
 
-  start_ = std::chrono::high_resolution_clock::now();  // Reset the timer
+  start_ = node_->now();  // Reset the timer
   return BT::NodeStatus::SUCCESS;
 }
 

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -25,7 +25,7 @@
 
 #include "behaviortree_cpp_v3/decorator_node.h"
 
-#include "distance_controller.hpp"
+#include "nav2_behavior_tree/plugins/distance_controller.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp

@@ -24,7 +24,7 @@
 
 #include "../../test_transform_handler.hpp"
 #include "../../test_dummy_tree_node.hpp"
-#include "../../../plugins/decorator/distance_controller.hpp"
+#include "nav2_behavior_tree/plugins/distance_controller.hpp"
 
 class DistanceControllerTestFixture : public ::testing::Test
 {