Added decorator node for checking stuck state

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<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"));
@@ -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;
 }