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

finished toid_interaction node

Browse Source
This commit is contained in:
Pimpest
2026-04-03 21:00:57 +02:00
parent 26ed379300
commit 038568c7e3
28 changed files with 708 additions and 407 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
toid_bt/behavior_trees/behavior1.xml
View File

@@ -1,5 +1,14 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="seq">
<Sequence>
<Seq1 service_name=""/>
<Seq2 text="0101"
service_name=""/>
<Seq3 service_name=""/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="seq1">
<Sequence>
<ZeroOdom service_name=""/>
@@ -70,6 +79,20 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="Seq1">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="Seq2">
<input_port name="text"
type="std::string"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="Seq3">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="TranslateX">
<input_port name="x"
type="double"/>

13
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -26,14 +26,14 @@
action_name=""/>
</Sequence>
<ForceSuccess>
<Timeout msec="13000">
<DetectStuck timeout="1.000000">
<MovePointSimple x="-1.0"
y="0"
theta="0"
max_speed="0.070000"
backwards="true"
action_name=""/>
</Timeout>
</DetectStuck>
</ForceSuccess>
<SetWidth width="{width}"
count="1"
@@ -72,14 +72,14 @@
action_name=""/>
</Sequence>
<ForceSuccess>
<Timeout msec="9000">
<DetectStuck timeout="1.000000">
<MovePointSimple x="-0.2"
y="0"
theta="0"
max_speed="0.05"
backwards="true"
action_name=""/>
</Timeout>
</DetectStuck>
</ForceSuccess>
<Sleep msec="1000"/>
<EndCalib service_name=""/>
@@ -102,6 +102,11 @@
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Decorator ID="DetectStuck">
<input_port name="timeout"
default="1.000000"
type="double"/>
</Decorator>
<Action ID="EndCalib">
<input_port name="service_name"
type="std::string">Service name</input_port>

42
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -5,45 +5,55 @@
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Decorator ID="DetectStuck">
<input_port name="timeout" type="double" default="1.000000"/>
<input_port name="timeout" default="1.000000" type="double"/>
</Decorator>
<Action ID="EndCalib">
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="service_name" type="std::string">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" 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>
<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>
</Action>
<Action ID="RotateSimple">
<input_port name="angle" type="double"/>
<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>
<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>
</Action>
<Action ID="RotateTowards">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<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>
<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>
</Action>
<Action ID="Seq1">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="Seq2">
<input_port name="text" type="std::string"/>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="Seq3">
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="SetWidth">
<input_port name="width" type="double"/>
<input_port name="count" type="int" default="1"/>
<input_port name="count" default="1" type="int"/>
<output_port name="new_width" type="double"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="TranslateX">
<input_port name="x" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="ZeroOdom">
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

43
toid_bt/include/toid_bt/plugins/send_text_action.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include "angles/angles.h"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "tf2/utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "toid_bt/plugin.hpp"
#include "toid_msgs/srv/send_string.hpp"
namespace toid
{
class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
{
public:
SendTextNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("text"),
});
}
bool setRequest(typename Request::SharedPtr &req) override {
std::string text = getInput<std::string>("text").value_or("");
req->text = text;
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
{
return BT::NodeStatus::SUCCESS;
}
};
} // namespace toid

9
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/send_text_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp"
#include "toid_bt/plugins/translate_x_action.hpp"
@@ -61,6 +62,14 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
BT::RosNodeParams service_params(nh, "/sequence2");
service_params.server_timeout = std::chrono::seconds(15);
factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
std::cout << describeCustomNodes() << std::endl;