add humble-navigation2

navigation2/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -0,0 +1,86 @@
+// 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.
+
+#include <string>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+DistanceTraveledCondition::DistanceTraveledCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  distance_(1.0),
+  transform_tolerance_(0.1),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
+{
+  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");
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+}
+
+BT::NodeStatus DistanceTraveledCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    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_,
+      transform_tolerance_))
+  {
+    RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Get euclidean distance
+  auto travelled = nav2_util::geometry_utils::euclidean_distance(
+    start_pose_.pose, current_pose.pose);
+
+  if (travelled < distance_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Update start pose
+  start_pose_ = current_pose;
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>("DistanceTraveled");
+}

navigation2/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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.
+
+#include <vector>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  first_time(true)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+BT::NodeStatus GloballyUpdatedGoalCondition::tick()
+{
+  if (first_time) {
+    first_time = false;
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>("GlobalUpdatedGoal");
+}

navigation2/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -0,0 +1,96 @@
+// 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.
+
+#include <string>
+#include <memory>
+
+#include "nav2_util/robot_utils.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/node_utils.hpp"
+
+#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GoalReachedCondition::GoalReachedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  initialized_(false),
+  global_frame_("map"),
+  robot_base_frame_("base_link")
+{
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
+}
+
+GoalReachedCondition::~GoalReachedCondition()
+{
+  cleanup();
+}
+
+BT::NodeStatus GoalReachedCondition::tick()
+{
+  if (!initialized_) {
+    initialize();
+  }
+
+  if (isGoalReached()) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void GoalReachedCondition::initialize()
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  nav2_util::declare_parameter_if_not_declared(
+    node_, "goal_reached_tol",
+    rclcpp::ParameterValue(0.25));
+  node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
+
+  initialized_ = true;
+}
+
+bool GoalReachedCondition::isGoalReached()
+{
+  geometry_msgs::msg::PoseStamped current_pose;
+
+  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 false;
+  }
+
+  geometry_msgs::msg::PoseStamped goal;
+  getInput("goal", goal);
+  double dx = goal.pose.position.x - current_pose.pose.position.x;
+  double dy = goal.pose.position.y - current_pose.pose.position.y;
+
+  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+}

navigation2/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2020 Aitor Miguel Blanco
+//
+// 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.
+
+#include <string>
+#include <vector>
+#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GoalUpdatedCondition::GoalUpdatedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{}
+
+BT::NodeStatus GoalUpdatedCondition::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", goals_);
+    config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", goal_);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  config().blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>("goals", current_goals);
+  geometry_msgs::msg::PoseStamped current_goal;
+  config().blackboard->get<geometry_msgs::msg::PoseStamped>("goal", current_goal);
+
+  if (goal_ != current_goal || goals_ != current_goals) {
+    goal_ = current_goal;
+    goals_ = current_goals;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>("GoalUpdated");
+}

navigation2/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp

@@ -0,0 +1,34 @@
+// 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.
+
+#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node)
+{
+  auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received");
+  return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerSimpleCondition(
+    "InitialPoseReceived",
+    std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1));
+}

navigation2/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// 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.
+
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryChargingCondition::IsBatteryChargingCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  battery_topic_("/battery_status"),
+  is_battery_charging_(false)
+{
+  getInput("battery_topic", battery_topic_);
+  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  battery_sub_ = node->create_subscription<sensor_msgs::msg::BatteryState>(
+    battery_topic_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
+}
+
+BT::NodeStatus IsBatteryChargingCondition::tick()
+{
+  callback_group_executor_.spin_some();
+  if (is_battery_charging_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  is_battery_charging_ =
+    (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
+}

navigation2/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

@@ -0,0 +1,74 @@
+// 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.
+
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryLowCondition::IsBatteryLowCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  battery_topic_("/battery_status"),
+  min_battery_(0.0),
+  is_voltage_(false),
+  is_battery_low_(false)
+{
+  getInput("min_battery", min_battery_);
+  getInput("battery_topic", battery_topic_);
+  getInput("is_voltage", is_voltage_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
+    battery_topic_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
+}
+
+BT::NodeStatus IsBatteryLowCondition::tick()
+{
+  callback_group_executor_.spin_some();
+  if (is_battery_low_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  if (is_voltage_) {
+    is_battery_low_ = msg->voltage <= min_battery_;
+  } else {
+    is_battery_low_ = msg->percentage <= min_battery_;
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
+}

navigation2/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

@@ -0,0 +1,61 @@
+// Copyright (c) 2021 Joshua Wallace
+//
+// 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.
+
+#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
+#include <chrono>
+#include <memory>
+#include <string>
+
+namespace nav2_behavior_tree
+{
+
+IsPathValidCondition::IsPathValidCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
+
+  server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+}
+
+BT::NodeStatus IsPathValidCondition::tick()
+{
+  nav_msgs::msg::Path path;
+  getInput("path", path);
+
+  auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
+
+  request->path = path;
+  auto result = client_->async_send_request(request);
+
+  if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    if (result.get()->is_valid) {
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsPathValidCondition>("IsPathValid");
+}

navigation2/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp

@@ -0,0 +1,150 @@
+// Copyright (c) 2018 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.
+
+#include <string>
+#include <chrono>
+
+#include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
+
+using namespace std::chrono_literals; // NOLINT
+
+namespace nav2_behavior_tree
+{
+
+IsStuckCondition::IsStuckCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  is_stuck_(false),
+  odom_history_size_(10),
+  current_accel_(0.0),
+  brake_accel_limit_(-10.0)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+  callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();});
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
+    "odom",
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1),
+    sub_option);
+
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node");
+
+  RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry");
+}
+
+IsStuckCondition::~IsStuckCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
+  callback_group_executor_.cancel();
+  callback_group_executor_thread.join();
+}
+
+void IsStuckCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  RCLCPP_INFO_ONCE(node_->get_logger(), "Got odometry");
+
+  while (odom_history_.size() >= odom_history_size_) {
+    odom_history_.pop_front();
+  }
+
+  odom_history_.push_back(*msg);
+
+  // TODO(orduno) #383 Move the state calculation and is stuck to robot class
+  updateStates();
+}
+
+BT::NodeStatus IsStuckCondition::tick()
+{
+  // TODO(orduno) #383 Once check for is stuck and state calculations are moved to robot class
+  //              this becomes
+  // if (robot_state_.isStuck()) {
+
+  if (is_stuck_) {
+    logStuck("Robot got stuck!");
+    return BT::NodeStatus::SUCCESS;  // Successfully detected a stuck condition
+  }
+
+  logStuck("Robot is free");
+  return BT::NodeStatus::FAILURE;  // Failed to detected a stuck condition
+}
+
+void IsStuckCondition::logStuck(const std::string & msg) const
+{
+  static std::string prev_msg;
+
+  if (msg == prev_msg) {
+    return;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str());
+  prev_msg = msg;
+}
+
+void IsStuckCondition::updateStates()
+{
+  // Approximate acceleration
+  // TODO(orduno) #400 Smooth out velocity history for better accel approx.
+  if (odom_history_.size() > 2) {
+    auto curr_odom = odom_history_.end()[-1];
+    double curr_time = static_cast<double>(curr_odom.header.stamp.sec);
+    curr_time += (static_cast<double>(curr_odom.header.stamp.nanosec)) * 1e-9;
+
+    auto prev_odom = odom_history_.end()[-2];
+    double prev_time = static_cast<double>(prev_odom.header.stamp.sec);
+    prev_time += (static_cast<double>(prev_odom.header.stamp.nanosec)) * 1e-9;
+
+    double dt = curr_time - prev_time;
+    double vel_diff = static_cast<double>(
+      curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x);
+    current_accel_ = vel_diff / dt;
+  }
+
+  is_stuck_ = isStuck();
+}
+
+bool IsStuckCondition::isStuck()
+{
+  // TODO(orduno) #400 The robot getting stuck can result on different types of motion
+  // depending on the state prior to getting stuck (sudden change in accel, not moving at all,
+  // random oscillations, etc). For now, we only address the case where there is a sudden
+  // harsh deceleration. A better approach to capture all situations would be to do a forward
+  // simulation of the robot motion and compare it with the actual one.
+
+  // Detect if robot bumped into something by checking for abnormal deceleration
+  if (current_accel_ < brake_accel_limit_) {
+    RCLCPP_DEBUG(
+      node_->get_logger(), "Current deceleration is beyond brake limit."
+      " brake limit: %.2f, current accel: %.2f", brake_accel_limit_, current_accel_);
+
+    return true;
+  }
+
+  return false;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck");
+}

navigation2/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// 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.
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+PathExpiringTimerCondition::PathExpiringTimerCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  period_(1.0),
+  first_time_(true)
+{
+  getInput("seconds", period_);
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+}
+
+BT::NodeStatus PathExpiringTimerCondition::tick()
+{
+  if (first_time_) {
+    getInput("path", prev_path_);
+    first_time_ = false;
+    start_ = node_->now();
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Grab the new path
+  nav_msgs::msg::Path path;
+  getInput("path", path);
+
+  // Reset timer if the path has been updated
+  if (prev_path_ != path) {
+    prev_path_ = path;
+    start_ = node_->now();
+  }
+
+  // Determine how long its been since we've started this iteration
+  auto elapsed = node_->now() - start_;
+
+  // Now, get that in seconds
+  auto seconds = elapsed.seconds();
+
+  if (seconds < period_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  start_ = node_->now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PathExpiringTimerCondition>("PathExpiringTimer");
+}

navigation2/nav2_behavior_tree/plugins/condition/time_expired_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.
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+
+#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+TimeExpiredCondition::TimeExpiredCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  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_ = node_->now();
+    return BT::NodeStatus::FAILURE;
+  }
+
+  // Determine how long its been since we've started this iteration
+  auto elapsed = node_->now() - start_;
+
+  // Now, get that in seconds
+  auto seconds = elapsed.seconds();
+
+  if (seconds < period_) {
+    return BT::NodeStatus::FAILURE;
+  }
+
+  start_ = node_->now();  // Reset the timer
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>("TimeExpired");
+}

navigation2/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp

@@ -0,0 +1,81 @@
+// Copyright (c) 2020 Samsung Research America
+//
+// 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.
+
+#include <string>
+#include <chrono>
+#include <memory>
+
+#include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
+
+using namespace std::chrono_literals; // NOLINT
+
+namespace nav2_behavior_tree
+{
+
+TransformAvailableCondition::TransformAvailableCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  was_found_(false)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  getInput("child", child_frame_);
+  getInput("parent", parent_frame_);
+
+  if (child_frame_.empty() || parent_frame_.empty()) {
+    RCLCPP_FATAL(
+      node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",
+      child_frame_.c_str(), parent_frame_.c_str());
+    exit(-1);
+  }
+
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");
+}
+
+TransformAvailableCondition::~TransformAvailableCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node");
+}
+
+BT::NodeStatus TransformAvailableCondition::tick()
+{
+  if (was_found_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  std::string tf_error;
+  bool found = tf_->canTransform(
+    child_frame_, parent_frame_, tf2::TimePointZero, &tf_error);
+
+  if (found) {
+    was_found_ = true;
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  RCLCPP_INFO(
+    node_->get_logger(), "Transform from %s to %s was not found, tf error: %s",
+    child_frame_.c_str(), parent_frame_.c_str(), tf_error.c_str());
+
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::TransformAvailableCondition>("TransformAvailable");
+}