diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index f4ed8bd..38e29f7 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -23,16 +23,23 @@
-
+
+
+
+
+
+
diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj
index 52ae4ff..501f4ea 100644
--- a/toid_bt/behavior_trees/toid_behaviors.btproj
+++ b/toid_bt/behavior_trees/toid_behaviors.btproj
@@ -4,43 +4,46 @@
+
+
+
- Service name
+ Service name
-
-
- Action server name
+
+
+ Action server name
-
-
- Action server name
+
+
+ Action server name
-
-
- Action server name
+
+
+ Action server name
-
+
- Service name
+ Service name
-
- Action server name
+
+ Action server name
- Service name
+ Service name
diff --git a/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp b/toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp
new file mode 100644
index 0000000..8430f31
--- /dev/null
+++ b/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("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("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
\ No newline at end of file
diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp
index b9eff34..5db62cf 100644
--- a/toid_bt/src/bt_executor.cpp
+++ b/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
@@ -22,7 +23,6 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
*/
tf_buffer_ = std::make_shared(node()->get_clock());
tf_listener_ = std::make_shared(*tf_buffer_);
-
nav2_util::declare_parameter_if_not_declared(
node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
@@ -61,6 +61,8 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
+ factory.registerNodeType("DetectStuck", get_pose);
+
std::cout << describeCustomNodes() << std::endl;
}