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

added behavior trees

Browse Source
This commit is contained in:
Pimpest
2026-04-17 01:38:55 +02:00
parent 0a0c6da6f0
commit 50ec8e3a83
5 changed files with 100 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
toid_bt/behavior_trees/behavior1.xml
View File

@@ -6,7 +6,7 @@
<Sleep msec="999999"/> <Sleep msec="999999"/>
</InPose> </InPose>
<Seq1 service_name=""/> <Seq1 service_name=""/>
<Seq2 text="0101" <Seq2 text="1010"
service_name=""/> service_name=""/>
<Seq3 service_name=""/> <Seq3 service_name=""/>
</Sequence> </Sequence>

2
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -47,7 +47,7 @@
<Sleep msec="5000"/> <Sleep msec="5000"/>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<Sleep msec="1000"/> <Sleep msec="1000"/>
<Repeat num_cycles="6"> <Repeat num_cycles="2">
<Sequence> <Sequence>
<MovePointSimple x="1.1" <MovePointSimple x="1.1"
y="0" y="0"

61
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -8,56 +8,63 @@
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="y" type="double"/> <input_port name="y" type="double"/>
<input_port name="theta" type="double"/> <input_port name="theta" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="backwards" type="bool" default="false"/> <input_port name="backwards" default="false" type="bool"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Decorator ID="DetectStuck"> <Decorator ID="DetectStuck">
<input_port name="timeout" type="double" default="1.000000"/> <input_port name="timeout" default="1.000000" type="double"/>
</Decorator> </Decorator>
<Action ID="EndCalib"> <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>
<Decorator ID="InPose"> <Decorator ID="InPose">
<input_port name="timeout" type="double" default="1.000000"/> <input_port name="timeout" default="1.000000" type="double"/>
</Decorator> </Decorator>
<Action ID="MovePointSimple"> <Action ID="MovePointSimple">
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="y" type="double"/> <input_port name="y" type="double"/>
<input_port name="theta" type="double"/> <input_port name="theta" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="backwards" type="bool" default="false"/> <input_port name="backwards" default="false" type="bool"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="RotateAcorns"> <Action ID="RotateAcorns">
<input_port name="angle" type="double"/> <input_port name="angle" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" type="double" default="0.000000"/> <input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="RotateSimple"> <Action ID="RotateSimple">
<input_port name="angle" type="double"/> <input_port name="angle" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" type="double" default="0.000000"/> <input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="RotateTowards"> <Action ID="RotateTowards">
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="y" type="double"/> <input_port name="y" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" type="double" default="0.000000"/> <input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="Seq1"> <Action ID="Seq1">
<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>
<Action ID="Seq2"> <Action ID="Seq2">
<input_port name="text" type="std::string"/> <input_port name="text" type="std::string"/>
<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>
<Action ID="Seq3"> <Action ID="Seq3">
<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>
<Condition ID="SetInitialPose">
<input_port name="y" type="double" default="0.000000">Y position in meters</input_port>
<input_port name="theta" type="double" default="0.000000">Heading in degrees</input_port>
<input_port name="frame_id" type="std::string" default="map">Reference frame</input_port>
<input_port name="x" type="double" default="0.000000">X position in meters</input_port>
<input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>
</Condition>
<Action ID="SetParameter"> <Action ID="SetParameter">
<input_port name="parameter" type="std::string"/> <input_port name="parameter" type="std::string"/>
<input_port name="value" type="std::string"/> <input_port name="value" type="std::string"/>
@@ -65,20 +72,20 @@
</Action> </Action>
<Action ID="SetWidth"> <Action ID="SetWidth">
<input_port name="width" type="double"/> <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"/> <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>
<Action ID="TranslateX"> <Action ID="TranslateX">
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="max_speed" type="double" default="0.000000"/> <input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="WaitPullPin"> <Action ID="WaitPullPin">
<input_port name="action_name" type="std::string" default="">Action server name</input_port> <input_port name="action_name" type="std::string">Action server name</input_port>
</Action> </Action>
<Action ID="ZeroOdom"> <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> </Action>
</TreeNodesModel> </TreeNodesModel>
</root> </root>

60
toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp Normal file
View File

@@ -0,0 +1,60 @@
#pragma once
#include "angles/angles.h"
#include "behaviortree_ros2/bt_topic_pub_node.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace toid
{
class SetInitialPoseNode
: public BT::RosTopicPubNode<geometry_msgs::msg::PoseWithCovarianceStamped>
{
public:
SetInitialPoseNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
: BT::RosTopicPubNode<geometry_msgs::msg::PoseWithCovarianceStamped>(name, conf, params)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x", 0.0, "X position in meters"),
BT::InputPort<double>("y", 0.0, "Y position in meters"),
BT::InputPort<double>("theta", 0.0, "Heading in degrees"),
BT::InputPort<std::string>("frame_id", "map", "Reference frame"),
});
}
bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override
{
double x = getInput<double>("x").value_or(0.0);
double y = getInput<double>("y").value_or(0.0);
double theta_deg = getInput<double>("theta").value_or(0.0);
std::string frame_id = getInput<std::string>("frame_id").value_or("map");
msg.header.stamp = node_->get_clock()->now();
msg.header.frame_id = frame_id;
msg.pose.pose.position.x = x;
msg.pose.pose.position.y = y;
msg.pose.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, angles::from_degrees(theta_deg));
msg.pose.pose.orientation = tf2::toMsg(q);
// Default covariance: small diagonal for x, y, yaw
msg.pose.covariance = {};
msg.pose.covariance[0] = 0.25; // x
msg.pose.covariance[7] = 0.25; // y
msg.pose.covariance[35] = 0.068; // yaw
return true;
}
};
} // namespace toid

4
toid_bt/src/bt_executor.cpp
View File

@@ -14,6 +14,7 @@
#include "toid_bt/plugins/send_text_action.hpp" #include "toid_bt/plugins/send_text_action.hpp"
#include "toid_bt/plugins/set_parameter_action.hpp" #include "toid_bt/plugins/set_parameter_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp" #include "toid_bt/plugins/stuck_detector_decorator.hpp"
#include "toid_bt/plugins/set_initial_pose_action.hpp"
#include "toid_bt/plugins/translate_x_action.hpp" #include "toid_bt/plugins/translate_x_action.hpp"
namespace toid namespace toid
@@ -84,6 +85,9 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::SetParameterNode>( factory.registerNodeType<toid::SetParameterNode>(
"SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters")); "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
factory.registerNodeType<toid::SetInitialPoseNode>(
"SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero")); factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib")); factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));