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

Behavior tree glue code

Browse Source
This commit is contained in:
Pimpest
2026-04-15 21:18:49 +02:00
parent 0a7d8525a0
commit d0ffceebe1
5 changed files with 156 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -4,59 +4,81 @@
<include path="calibration_routines.xml"/>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="ApproachAcorns">
<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>
</Action>
<Decorator ID="DetectStuck">
<input_port name="timeout" default="1.000000" type="double"/>
<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>
<Decorator ID="InPose">
<input_port name="timeout" default="1.000000" type="double"/>
<input_port name="timeout" type="double" default="1.000000"/>
</Decorator>
<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="RotateAcorns">
<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>
</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="Seq1">
<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="Seq2">
<input_port name="text" type="std::string"/>
<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="Seq3">
<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="SetParameter">
<input_port name="parameter" type="std::string"/>
<input_port name="value" type="std::string"/>
<input_port name="node" type="std::string"/>
</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="WaitPullPin">
<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>

48
toid_bt/include/toid_bt/plugins/pull_pin_action.hpp Normal file
View File

@@ -0,0 +1,48 @@
#pragma once
#include "angles/angles.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "tf2/utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "toid_bt/plugin.hpp"
#include "toid_msgs/action/empty_action.hpp"
namespace toid
{
class PullPinNode : public BT::RosActionNode<toid_msgs::action::EmptyAction>
{
public:
PullPinNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosActionNode<toid_msgs::action::EmptyAction>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal &) override
{
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult &) override
{
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
{
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
} // namespace toid

50
toid_bt/include/toid_bt/plugins/set_parameter_action.hpp Normal file
View File

@@ -0,0 +1,50 @@
#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"
#include "rcl_interfaces/srv/set_parameters.hpp"
namespace toid
{
class SetParameterNode : public BT::RosServiceNode<rcl_interfaces::srv::SetParameters>
{
public:
SetParameterNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosServiceNode<rcl_interfaces::srv::SetParameters>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("node"),
BT::InputPort<std::string>("parameter"),
BT::InputPort<std::string>("value")
});
}
bool setRequest(typename Request::SharedPtr & request) override {
std::string parameter = getInput<std::string>("parameter").value();
std::string value = getInput<std::string>("value").value();
std::string node = getInput<std::string>("node").value();
setServiceName("/" + node + "/set_parameters");
request->parameters.emplace_back();
request->parameters.front().name = parameter;
request->parameters.front().value.string_value = value;
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
{
return BT::NodeStatus::SUCCESS;
}
};
} // namespace

13
toid_bt/src/bt_executor.cpp
View File

@@ -8,9 +8,11 @@
#include "toid_bt/plugins/end_calib_action.hpp"
#include "toid_bt/plugins/in_position_decorator.hpp"
#include "toid_bt/plugins/move_coords_action.hpp"
#include "toid_bt/plugins/pull_pin_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/set_parameter_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp"
#include "toid_bt/plugins/translate_x_action.hpp"
@@ -59,9 +61,15 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::MovePointNode>(
"MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
factory.registerNodeType<toid::MovePointNode>(
"ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
factory.registerNodeType<toid::RotateNode>(
"RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
factory.registerNodeType<toid::RotateNode>(
"RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
factory.registerNodeType<toid::RotateTowardsNode>(
"RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
@@ -71,6 +79,11 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::EndCalibNode>(
"SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
factory.registerNodeType<toid::SetParameterNode>(
"SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));

4
toid_interaction/toid_interaction/interaction_node.py
View File

@@ -107,7 +107,9 @@ def main(args=None):
rclpy.init(args=args)
node = InteracitionNode()
rclpy.executors.MultiThreadedExecutor().spin(node)
executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
rclpy.shutdown()