Added decorator node for checking stuck state

toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+
+namespace toid
+{
+
+class StuckDetectorNode : public BT::DecoratorNode
+{
+public:
+  StuckDetectorNode(
+    const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose)
+  : BT::DecoratorNode(name, conf), get_pose(get_pose)
+  {
+  }
+
+  static BT::PortsList providedPorts() {
+    return {
+        BT::InputPort<double>("timeout", 1, {})
+    };
+  }
+
+  private:
+    bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) {
+        const double dx = poseA.pose.position.x - poseB.pose.position.x;
+        const double dy = poseA.pose.position.y - poseB.pose.position.y;
+        const double dth = abs(tf2::getYaw(poseA.pose.orientation) - tf2::getYaw(poseB.pose.orientation));
+        return dx*dx + dy*dy < 0.004*0.004 && dth < 0.05;
+    }
+
+    BT::NodeStatus tick() override
+    {
+        if(status() == BT::NodeStatus::IDLE) {
+            setStatus(BT::NodeStatus::RUNNING);
+            last_pos_change = clock.now();
+        }
+
+        double timeout = getInput<double>("timeout").value_or(1.0);
+
+
+        geometry_msgs::msg::PoseStamped pose;
+        get_pose(pose);
+
+        if(checkIfPosesAreClose(last_pos, pose)) {
+            if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
+                haltChild();
+                return BT::NodeStatus::FAILURE;
+            }
+        } else {
+            last_pos = pose;
+            last_pos_change = clock.now();
+        }
+
+        const BT::NodeStatus child_status = child_node_->executeTick();
+        return child_status;
+    }
+
+  geometry_msgs::msg::PoseStamped last_pos;
+  PoseFunc get_pose;
+  rclcpp::Time last_pos_change;
+  rclcpp::Clock clock;
+};
+
+}  // namespace toid