toid_bt/behavior_trees/behavior1.xml

@@ -23,16 +23,23 @@
 
   <BehaviorTree ID="test_1">
     <Sequence>
-      <RotateTowards x="0.4"
+      <DetectStuck timeout="1.000000">
-                     y="0.0"
+        <RotateTowards x="0.4"
-                     max_speed="0.000000"
+                       y="0.0"
-                     min_angle="0.000000"
+                       max_speed="0.000000"
-                     action_name=""/>
+                       min_angle="0.000000"
+                       action_name=""/>
+      </DetectStuck>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Decorator ID="DetectStuck">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
     <Action ID="MovePointSimple">
       <input_port name="x"
                   type="double"/>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,43 +4,46 @@
     <include path="calibration_routines.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
+        <Decorator ID="DetectStuck">
+            <input_port name="timeout" type="double" default="1.000000"/>
+        </Decorator>
         <Action ID="EndCalib">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
             <input_port name="theta" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="backwards" type="bool" default="false"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
-            <input_port name="count" default="1" type="int"/>
+            <input_port name="count" type="int" default="1"/>
             <output_port name="new_width" type="double"/>
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Action ID="TranslateX">
             <input_port name="x" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="ZeroOdom">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
     </TreeNodesModel>
 </root>

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

toid_bt/src/bt_executor.cpp

@@ -9,6 +9,7 @@
 #include "toid_bt/plugins/move_coords_action.hpp"
 #include "toid_bt/plugins/rotate_action.hpp"
 #include "toid_bt/plugins/rotate_towards_action.hpp"
+#include "toid_bt/plugins/stuck_detector_decorator.hpp"
 #include "toid_bt/plugins/translate_x_action.hpp"
 
 namespace toid
@@ -23,7 +24,6 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
-
   nav2_util::declare_parameter_if_not_declared(
     node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
   node()->get_parameter("base_frame", base_frame_);
@@ -61,6 +61,8 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
+  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
+
   std::cout << describeCustomNodes() << std::endl;
 }