Add condition nodes and behavior tree to enable replan on new goal

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

nav2_behavior_tree/CMakeLists.txt

@@ -77,6 +77,15 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node)
 add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp)
 list(APPEND plugin_libs nav2_goal_reached_condition_bt_node)
 
+add_library(nav2_is_new_goal_condition_bt_node SHARED plugins/condition/is_new_goal_condition.cpp)
+list(APPEND plugin_libs nav2_is_new_goal_condition_bt_node)
+
+add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp)
+list(APPEND plugin_libs nav2_time_expired_condition_bt_node)
+
+add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp)
+list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
+
 add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
 list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -0,0 +1,114 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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__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"
+
+namespace nav2_behavior_tree
+{
+
+class DistanceTraveledCondition : public BT::ConditionNode
+{
+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;
+    }
+
+    // Determine distance travelled since we've started this iteration
+    geometry_msgs::msg::PoseStamped current_pose;
+    if (!nav2_util::getCurrentPose(current_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;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("distance", 1.0, "Distance")
+    };
+  }
+
+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);
+  }
+
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  geometry_msgs::msg::PoseStamped start_pose_;
+
+  double distance_;
+  bool first_time_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/is_new_goal_condition.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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__IS_NEW_GOAL_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__IS_NEW_GOAL_CONDITION_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class IsNewGoalCondition : public BT::ConditionNode
+{
+public:
+  IsNewGoalCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf)
+  : BT::ConditionNode(condition_name, conf)
+  {
+    config().blackboard->set("new_goal_received", false);
+  }
+
+  IsNewGoalCondition() = delete;
+
+  BT::NodeStatus tick() override
+  {
+    // Check if there's a new goal on the blackboard
+    if (config().blackboard->get<bool>("new_goal_received")) {
+      // Reset the flag in the blackboard
+      config().blackboard->set("new_goal_received", false);
+      return BT::NodeStatus::SUCCESS;
+    }
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {};
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsNewGoalCondition>("IsNewGoal");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__IS_NEW_GOAL_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/time_expired_condition.cpp

@@ -0,0 +1,89 @@
+// Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// 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__TIME_EXPIRED_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_
+
+#include <chrono>
+#include <string>
+
+#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)
+  : 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_;
+
+    // Now, get that in seconds
+    typedef std::chrono::duration<float> float_seconds;
+    auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
+
+    if (seconds.count() < period_) {
+      return BT::NodeStatus::FAILURE;
+    }
+
+    start_ = std::chrono::high_resolution_clock::now();  // Reset the timer
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("hz", 10.0, "Rate")
+    };
+  }
+
+private:
+  std::chrono::time_point<std::chrono::high_resolution_clock> start_;
+  double period_;
+  bool first_time_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired");
+}
+
+#endif  // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_

nav2_bringup/bringup/launch/tb3_simulation_launch.py

@@ -87,7 +87,7 @@ def generate_launch_description():
         'bt_xml_file',
         default_value=os.path.join(
             get_package_share_directory('nav2_bt_navigator'),
-            'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
+            'behavior_trees', 'navigate_with_immediate_replanning.xml'),
         description='Full path to the behavior tree xml file to use')
 
     declare_autostart_cmd = DeclareLaunchArgument(

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -49,7 +49,7 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_xml_filename: "navigate_with_immediate_replanning.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node
@@ -65,6 +65,9 @@ bt_navigator:
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_is_new_goal_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -49,7 +49,7 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_xml_filename: "navigate_with_immediate_replanning.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node
@@ -65,6 +65,9 @@ bt_navigator:
     - nav2_recovery_node_bt_node
     - nav2_pipeline_sequence_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_is_new_goal_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bringup/bringup/params/nav2_params.yaml

@@ -49,7 +49,7 @@ amcl_rclcpp_node:
 bt_navigator:
   ros__parameters:
     use_sim_time: True
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_xml_filename: "navigate_with_immediate_replanning.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node
@@ -66,6 +66,9 @@ bt_navigator:
     - nav2_pipeline_sequence_bt_node
     - nav2_round_robin_node_bt_node
     - nav2_transform_available_condition_bt_node
+    - nav2_is_new_goal_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:

nav2_bt_navigator/behavior_trees/navigate_with_immediate_replanning.xml

@@ -0,0 +1,21 @@
+<!--
+  This Behavior Tree replans the global path at 0.5hz OR 2m distance OR when a new goal is available
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <PipelineSequence name="NavigateWithReplanning">
+      <Fallback>
+        <Inverter>
+          <Fallback>
+            <TimeExpired hz="0.5"/>
+            <DistanceTraveled distance="2.0"/>
+            <IsNewGoal/>
+          </Fallback>
+        </Inverter>
+        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+      </Fallback>
+      <FollowPath path="{path}" controller_id="FollowPath"/>
+    </PipelineSequence>
+  </BehaviorTree>
+</root>