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

add backwards to behavior tree action

Browse Source
This commit is contained in:
Pimpest
2026-03-20 17:05:30 +01:00
parent 9010bac9a0
commit 8f4f7dba8b
4 changed files with 94 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -1,40 +1,95 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="wheel_ratio_calibration">
<BehaviorTree ID="track_calib">
<Sequence>
<Delay delay_msec="5000">
<RotateSimple angle="0"
max_speed="0.000000"
min_angle="270"
action_name=""/>
</Delay>
<SetBlackboard value="0.264"
output_key="width"/>
<Sleep msec="1000"/>
<ZeroOdom service_name=""/>
<Sequence>
<MovePointSimple x="1.0"
<Sleep msec="1000"/>
<MovePointSimple x="-0.4"
y="0"
theta="0"
max_speed="0.10000"
backwards="true"
action_name=""/>
<RotateSimple angle="180"
<Sleep msec="500"/>
<RotateSimple angle="0"
max_speed="0.500000"
min_angle="270"
action_name=""/>
</Sequence>
<ForceSuccess>
<Timeout msec="13000">
<MovePointSimple x="1.0"
y="0"
theta="0"
max_speed="0.070000"
backwards="false"
action_name=""/>
</Timeout>
</ForceSuccess>
<SetWidth width="{width}"
count="1"
new_width="{width}"
service_name=""/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="wheel_ratio_calibration">
<Sequence>
<Sleep msec="5000"/>
<ZeroOdom service_name=""/>
<Sleep msec="1000"/>
<Sequence>
<MovePointSimple x="1.1"
y="0"
theta="0"
max_speed="0.10000"
backwards="false"
action_name=""/>
<Sleep msec="1000"/>
<RotateSimple angle="180"
max_speed="0.300000"
min_angle="10"
action_name=""/>
<MovePointSimple x="0.4"
<Sleep msec="500"/>
<MovePointSimple x="0.35"
y="0"
theta="180"
max_speed="0.100000"
backwards="false"
action_name=""/>
<RotateSimple angle="0"
max_speed="0.500000"
max_speed="0.300000"
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=""/>
<ForceSuccess>
<Timeout msec="9000">
<MovePointSimple x="-0.2"
y="0"
theta="0"
max_speed="0.05"
backwards="true"
action_name=""/>
</Timeout>
</ForceSuccess>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="wheel_size">
<Sequence>
<Sleep msec="1000"/>
<ZeroOdom service_name=""/>
<Sleep msec="1000"/>
<MovePointSimple x="-1.200"
y="0"
theta="0"
max_speed="0.250000"
backwards="true"
action_name=""/>
</Sequence>
</BehaviorTree>
@@ -50,6 +105,9 @@
<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>
@@ -65,14 +123,20 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="TranslateX">
<input_port name="x"
<Action ID="SetWidth">
<input_port name="width"
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="count"
default="1"
type="int"/>
<output_port name="new_width"
type="double"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Action ID="ZeroOdom">
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>

1
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -12,6 +12,7 @@
<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>
</Action>
<Action ID="RotateSimple">

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

@@ -28,6 +28,7 @@ public:
BT::InputPort<double>("y"),
BT::InputPort<double>("theta"),
BT::InputPort<double>("max_speed", 0, {}),
BT::InputPort<bool>("backwards", false, {}),
//BT::InputPort<double>("options"),
});
}
@@ -38,6 +39,7 @@ public:
auto y_goal = getInput<double>("y");
auto theta = getInput<double>("theta");
auto max_speed = getInput<double>("max_speed").value();
auto backwards = getInput<bool>("backwards").value();
goal.x = x_goal.value();
goal.y = y_goal.value();
@@ -50,6 +52,7 @@ public:
}
goal.max_speed = max_speed;
goal.backwards = backwards;
return true;
}