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

Modified behavior tree nodes to include max_speed

Browse Source
This commit is contained in:
pimpest
2026-03-07 15:40:41 +01:00
parent 77cb2c5573
commit 8ae7e4befd
9 changed files with 138 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
toid_bt/behavior_trees/behavior1.xml
View File

@@ -3,31 +3,15 @@
<BehaviorTree ID="test_1">
<Sequence>
<RotateSimple angle="90"
radians="false"
min_angle="0.000000"
action_name=""/>
<Sleep msec="1000"/>
<RotateSimple angle="0"
radians="false"
min_angle="0.000000"
action_name=""/>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="RotateSimple">
<input_port name="angle"
type="double"/>
<input_port name="radians"
default="false"
type="bool"/>
<input_port name="min_angle"
default="0.000000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
</TreeNodesModel>
</root>

44
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -3,19 +3,37 @@
<BehaviorTree ID="wheel_ratio_calibration">
<Sequence>
<Delay delay_msec="5000">
<ZeroOdom service_name=""/>
<RotateSimple angle="0"
max_speed="0.000000"
min_angle="270"
action_name=""/>
</Delay>
<Sequence>
<MovePointSimple x="1.0"
y="0"
theta="0"
max_speed="0.10000"
action_name=""/>
<RotateSimple angle="180"
radians="false"
min_angle="0.000000"
max_speed="0.500000"
min_angle="10"
action_name=""/>
<MovePointSimple x="0.4"
y="0"
theta="180"
max_speed="0.100000"
action_name=""/>
<RotateSimple angle="0"
max_speed="0.500000"
min_angle="-10"
action_name=""/>
</Sequence>
<TranslateX x="-0.5"
max_speed="0.050000"
action_name=""/>
<RotateSimple angle="0"
max_speed="0.000000"
min_angle="-270"
action_name=""/>
</Sequence>
</BehaviorTree>
@@ -29,24 +47,32 @@
type="double"/>
<input_port name="theta"
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>
</Action>
<Action ID="RotateSimple">
<input_port name="angle"
type="double"/>
<input_port name="radians"
default="false"
type="bool"/>
<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="ZeroOdom">
<input_port name="service_name"
type="std::string">Service name</input_port>
<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>
</Action>
</TreeNodesModel>

10
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -11,18 +11,19 @@
<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="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateSimple">
<input_port name="angle" type="double"/>
<input_port name="radians" default="false" type="bool"/>
<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="radians" default="false" type="bool"/>
<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>
@@ -32,6 +33,11 @@
<output_port name="new_width" type="double"/>
<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" 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">Service name</input_port>
</Action>

3
toid_bt/include/toid_bt/plugins/end_calib_action.hpp
View File

@@ -23,7 +23,8 @@ public:
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("width"), BT::InputPort<int>("count", 1, {}),
BT::InputPort<double>("width"),
BT::InputPort<int>("count", 1, {}),
BT::OutputPort<double>("new_width"),
//BT::InputPort<double>("options"),
});

10
toid_bt/include/toid_bt/plugins/move_coords_action.hpp
View File

@@ -24,7 +24,10 @@ public:
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("theta"),
BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
BT::InputPort<double>("theta"),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
@@ -34,6 +37,8 @@ public:
auto x_goal = getInput<double>("x");
auto y_goal = getInput<double>("y");
auto theta = getInput<double>("theta");
auto max_speed = getInput<double>("max_speed").value();
goal.x = x_goal.value();
goal.y = y_goal.value();
if (!theta.has_value()) {
@@ -43,6 +48,9 @@ public:
} else {
goal.theta = angles::from_degrees(theta.value_or(0));
}
goal.max_speed = max_speed;
return true;
}

7
toid_bt/include/toid_bt/plugins/rotate_action.hpp
View File

@@ -25,7 +25,7 @@ public:
return providedBasicPorts({
BT::InputPort<double>("angle", {}),
BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<bool>("radians", false, {}),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
@@ -34,13 +34,12 @@ public:
{
auto angle = getInput<double>("angle");
auto min_angle = getInput<double>("min_angle");
auto radians = getInput<bool>("radians");
auto max_speed = getInput<double>("max_speed");
goal.angle = angle.value();
goal.min_angle = min_angle.value();
if (!radians.value()) {
goal.max_speed = max_speed.value();
goal.angle = angles::from_degrees(goal.angle);
goal.min_angle = angles::from_degrees(goal.min_angle);
}
return true;
}

13
toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
View File

@@ -26,7 +26,7 @@ public:
BT::InputPort<double>("x", {}),
BT::InputPort<double>("y", {}),
BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<bool>("radians", false, {}),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
@@ -35,18 +35,15 @@ public:
{
auto x = getInput<double>("x").value();
auto y = getInput<double>("y").value();
auto min_angle = getInput<double>("min_angle");
auto radians = getInput<bool>("radians");
auto min_angle = getInput<double>("min_angle").value();
auto max_speed = getInput<double>("max_speed").value();
geometry_msgs::msg::PoseStamped pose;
get_pose(pose);
goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
goal.min_angle = min_angle.value();
if (!radians.value()) {
goal.min_angle = angles::from_degrees(goal.min_angle);
}
goal.min_angle = angles::from_degrees(min_angle);
goal.max_speed = max_speed;
return true;
}

59
toid_bt/include/toid_bt/plugins/translate_x_action.hpp Normal file
View File

@@ -0,0 +1,59 @@
#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/simple_translate_x.hpp"
namespace toid
{
class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleTranslateX>
{
public:
TranslateXNode(
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
PoseFunc pose)
: BT::RosActionNode<toid_msgs::action::SimpleTranslateX>(name, conf, params), get_pose(pose)
{
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("x"),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
});
}
bool setGoal(Goal & goal) override
{
auto x_goal = getInput<double>("x").value();
auto max_speed = getInput<double>("max_speed").value();
goal.distance = x_goal;
goal.max_speed = max_speed;
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult & wr) override
{
return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
{
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
PoseFunc get_pose;
};
} // namespace toid

11
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/translate_x_action.hpp"
namespace toid
{
@@ -49,17 +50,17 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType<toid::RotateTowardsNode>(
"RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
factory.registerNodeType<toid::TranslateXNode>(
"TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);
factory.registerNodeType<toid::EndCalibNode>(
"SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
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"));
std::cout << describeCustomNodes() << std::endl;
}
void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)