Fix time expired and distance traveled conditions

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -16,92 +16,57 @@
 #ifndef NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
 #define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_
 
-#include <string>
-#include <memory>
-
-#include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
 #include "nav2_util/robot_utils.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "tf2_ros/buffer.h"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "distance_traveled_condition.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class DistanceTraveledCondition : public BT::ConditionNode
+DistanceTraveledCondition::DistanceTraveledCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  distance_(1.0),
+  first_tick_(true)
 {
-public:
-  DistanceTraveledCondition(
-    const std::string & condition_name,
-    const BT::NodeConfiguration & conf)
-  : BT::ConditionNode(condition_name, conf),
-    distance_(1.0),
-    first_time_(true)
-  {
-    getInput("distance", distance_);
-    node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-    tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
-  }
-
-  DistanceTraveledCondition() = delete;
-
-  BT::NodeStatus tick() override
-  {
-    if (first_time_) {
-      if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
-        RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
-        return BT::NodeStatus::FAILURE;
-      }
-      first_time_ = false;
-      return BT::NodeStatus::SUCCESS;
-    }
+  getInput("distance", distance_);
+  getInput("global_frame", global_frame_);
+  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");
+}
 
-    // Determine distance travelled since we've started this iteration
-    geometry_msgs::msg::PoseStamped current_pose;
-    if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
+BT::NodeStatus DistanceTraveledCondition::tick()
+{
+  if (first_tick_) {
+    if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
-      return BT::NodeStatus::FAILURE;
     }
-
-    // Get euclidean distance
-    auto travelled = euclidean_distance(start_pose_, current_pose);
-
-    if (travelled < distance_) {
-      return BT::NodeStatus::FAILURE;
-    }
-
-    // Update start pose
-    start_pose_ = current_pose;
-
-    return BT::NodeStatus::SUCCESS;
+    return BT::NodeStatus::FAILURE;
   }
 
-  static BT::PortsList providedPorts()
-  {
-    return {
-      BT::InputPort<double>("distance", 1.0, "Distance")
-    };
+  // 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_)) {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
   }
 
-private:
-  double euclidean_distance(
-    const geometry_msgs::msg::PoseStamped & pose1,
-    const geometry_msgs::msg::PoseStamped & pose2)
-  {
-    const double dx = pose1.pose.position.x - pose2.pose.position.x;
-    const double dy = pose1.pose.position.y - pose2.pose.position.y;
-    const double dz = pose1.pose.position.z - pose2.pose.position.z;
-    return std::sqrt(dx * dx + dy * dy + dz * dz);
-  }
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
 
-  rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<tf2_ros::Buffer> tf_;
+  if (travelled < distance_) {
+    return BT::NodeStatus::FAILURE;
+  }
 
-  geometry_msgs::msg::PoseStamped start_pose_;
+  // Update start pose
+  start_pose_ = current_pose;
 
-  double distance_;
-  bool first_time_;
-};
+  return BT::NodeStatus::SUCCESS;
+}
 
 }  // namespace nav2_behavior_tree
 

nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRAVELED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRAVELED_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "tf2_ros/buffer.h"
+
+namespace nav2_behavior_tree
+{
+
+class DistanceTraveledCondition : public BT::ConditionNode
+{
+public:
+  DistanceTraveledCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  DistanceTraveledCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("distance", 1.0, "Distance"),
+      BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  geometry_msgs::msg::PoseStamped start_pose_;
+
+  double distance_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
+  bool first_tick_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRAVELED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/time_expired_condition.cpp

@@ -21,62 +21,44 @@
 
 #include "behaviortree_cpp_v3/condition_node.h"
 
+#include "time_expired_condition.hpp"
+
 namespace nav2_behavior_tree
 {
 
-class TimeExpiredCondition : public BT::ConditionNode
+TimeExpiredCondition::TimeExpiredCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  period_(1.0),
+  first_tick_(true)
 {
-public:
-  TimeExpiredCondition(
-    const std::string & condition_name,
-    const BT::NodeConfiguration & conf)
-  : BT::ConditionNode(condition_name, conf),
-    first_time_(true)
-  {
-    double hz = 1.0;
-    getInput("hz", hz);
-    period_ = 1.0 / hz;
-  }
-
-  TimeExpiredCondition() = delete;
-
-  BT::NodeStatus tick() override
-  {
-    if (first_time_) {
-      start_ = std::chrono::high_resolution_clock::now();
-      first_time_ = false;
-      return BT::NodeStatus::SUCCESS;
-    }
-
-    // Determine how long its been since we've started this iteration
-    auto now = std::chrono::high_resolution_clock::now();
-    auto elapsed = now - start_;
+  getInput("seconds", period_);
+}
 
-    // Now, get that in seconds
-    typedef std::chrono::duration<float> float_seconds;
-    auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
+BT::NodeStatus TimeExpiredCondition::tick()
+{
+  if (first_tick_) {
+    start_ = std::chrono::high_resolution_clock::now();
+    first_tick_ = false;
+    return BT::NodeStatus::FAILURE;
+  }
 
-    if (seconds.count() < period_) {
-      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_;
 
-    start_ = std::chrono::high_resolution_clock::now();  // Reset the timer
-    return BT::NodeStatus::SUCCESS;
-  }
+  // Now, get that in seconds
+  typedef std::chrono::duration<float> float_seconds;
+  auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
 
-  // Any BT node that accepts parameters must provide a requiredNodeParameters method
-  static BT::PortsList providedPorts()
-  {
-    return {
-      BT::InputPort<double>("hz", 10.0, "Rate")
-    };
+  if (seconds.count() < period_) {
+    return BT::NodeStatus::FAILURE;
   }
 
-private:
-  std::chrono::time_point<std::chrono::high_resolution_clock> start_;
-  double period_;
-  bool first_time_;
-};
+  start_ = std::chrono::high_resolution_clock::now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
 
 }  // namespace nav2_behavior_tree
 

nav2_behavior_tree/plugins/condition/time_expired_condition.hpp

@@ -0,0 +1,53 @@
+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// 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_
+
+#include <chrono>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class TimeExpiredCondition : public BT::ConditionNode
+{
+public:
+  TimeExpiredCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  TimeExpiredCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("seconds", 1.0, "Seconds")
+    };
+  }
+
+private:
+  std::chrono::time_point<std::chrono::high_resolution_clock> start_;
+  double period_;
+  bool first_tick_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_