Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added decorator node for checking stuck state

Browse Source
This commit is contained in:
pimpest
2026-04-01 21:33:53 +02:00
parent ff1fbf5abe
commit 080676384d
4 changed files with 102 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
toid_bt/behavior_trees/behavior1.xml
View File

@@ -23,16 +23,23 @@
<BehaviorTree ID="test_1">
<Sequence>
<RotateTowards x="0.4"
y="0.0"
max_speed="0.000000"
min_angle="0.000000"
action_name=""/>
<DetectStuck timeout="1.000000">
<RotateTowards x="0.4"
y="0.0"
max_speed="0.000000"
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"/>

33
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -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="backwards" default="false" type="bool"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
<input_port name="max_speed" type="double" default="0.000000"/>
<input_port name="backwards" type="bool" default="false"/>
<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="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
<input_port name="max_speed" type="double" default="0.000000"/>
<input_port name="min_angle" type="double" default="0.000000"/>
<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="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
<input_port name="max_speed" type="double" default="0.000000"/>
<input_port name="min_angle" type="double" default="0.000000"/>
<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="action_name" type="std::string">Action server name</input_port>
<input_port name="max_speed" type="double" default="0.000000"/>
<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>

69
toid_bt/include/toid_bt/plugins/stuck_detector_decorator.hpp Normal file
View File

@@ -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

4
toid_bt/src/bt_executor.cpp
View File

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