Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

First set of behaviors

Browse Source
This commit is contained in:
Pimpest
2025-05-08 12:10:13 +02:00
parent da3d1d9d15
commit bce371dba2
13 changed files with 710 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -6,65 +6,86 @@
<TreeNodesModel>
<Action ID="CalibWidth">
<input_port name="PreviousWidth" type="double"/>
<input_port name="Count" type="int" default="1"/>
<input_port name="Count" default="1" type="int"/>
<output_port name="NewWidth" 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="CanChooser">
<output_port name="orient" type="int"/>
<output_port name="y_loc" type="double"/>
<output_port name="x_loc" type="double"/>
<input_port name="canlist" type="std::string"/>
</Action>
<Action ID="I2CSignal">
<input_port name="Address" type="int" default="42"/>
<input_port name="Data" type="int" default="0"/>
<input_port name="Address" default="42" type="int"/>
<input_port name="Data" default="0" type="int"/>
<output_port name="Result" type="int"/>
<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 ID="MovePoint">
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="tolerance" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="ornt_mult" type="double"/>
<input_port name="pos_mult" type="double"/>
<input_port name="t_ornt_mult" type="double"/>
<input_port name="obst_mult_c" type="double"/>
<input_port name="obst_mult_b" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="max_angular" type="double"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_wheel_speed" type="double"/>
<Action ID="LookAtNode">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveGlobal">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="6.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="4.000000" type="double"/>
<input_port name="ornt_mult" default="3.000000" type="double"/>
<input_port name="tolerance" default="0.050000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveLocal">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="2.000000" type="double"/>
<input_port name="pos_mult" default="15.000000" type="double"/>
<input_port name="ornt_mult" default="4.000000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="2.000000" type="double"/>
<input_port name="ornt_mult" default="4.000000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateNode">
<input_port name="angle" type="double"/>
<input_port name="pos_mult" type="double"/>
<input_port name="max_wheel_speed" type="double"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_angular" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="obst_mult_b" type="double"/>
<input_port name="obst_mult_c" type="double"/>
<input_port name="ornt_mult" type="double"/>
<input_port name="t_ornt_mult" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="tolerance" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="angle" type="double"/>
<input_port name="isDegree" type="bool" default="true"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="isDegree" default="true" type="bool"/>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic" type="int"/>
<input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>
<input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue" type="int"/>
<output_port name="IsBlue" type="bool"/>
<output_port name="tactic" type="int"/>
<input_port name="number" type="int"/>
</Action>
<Action ID="ZeroNode">
<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>

406
mg_bt/behavior_trees/tactics.xml
View File

@ -1,5 +1,260 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="pickup_can">
<Sequence>
<CanChooser orient="{can_orient}"
y_loc="{y_can}"
x_loc="{x_can}"
canlist="{target_can}"/>
<MoveGlobal x_goal="{x_can}"
y_goal="{y_can}"
max_wheel_speed="6.000000"
max_angular="3.140000"
max_vel="4.000000"
ornt_mult="3.000000"
tolerance="0.050000"
action_name="/MoveGlobal"/>
<LookAtNode x="{x_can}"
y="{y_can}"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/LookAt"/>
<MovePoint x_goal="{x_can}"
y_goal="{y_can}"
max_wheel_speed="5.000000"
max_angular="0.1"
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<Sequence>
<SubTree ID="pickup_right"
_skipIf="can_orient!=0"/>
<SubTree ID="pickup_down"
_skipIf="can_orient!=1"/>
<SubTree ID="pickup_left"
_skipIf="can_orient!=2"/>
<SubTree ID="pickup_up"
_skipIf="can_orient!=3"
_autoremap="false"/>
<MovePoint x_goal="0.8"
y_goal="0.6"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"
_skipIf="target_can!=1"/>
</Sequence>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_down">
<Sequence>
<RotateNode angle="-1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="-0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="-0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_left">
<Sequence>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="-0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="-3.14159265359"
max_wheel_speed="5.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="-0.12"
y_goal="0.0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.12"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_right">
<Sequence>
<RotateNode angle="-3.14159265359"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0.12"
y_goal="0.0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.12"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_up">
<Sequence>
<RotateNode angle="1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="-0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="-1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.0"
y_goal="-0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="tactic_base">
<Sequence>
<I2CSignal Address="66"
@ -29,23 +284,45 @@
<BehaviorTree ID="testbed">
<Sequence>
<SendPose x="1.0"
y="1.0"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<Delay delay_msec="5000">
<SendPose x="2.0"
y="0.5"
angle="-1"
isDegree="false"
service_name="/set_pose"/>
</Delay>
<SubTree ID="pickup_can"
target_can="0"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="1"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="2"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="3"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="6"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="8"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="5"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="4"
_autoremap="false"/>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="CanChooser">
<output_port name="orient"
type="int"/>
<output_port name="y_loc"
type="double"/>
<output_port name="x_loc"
type="double"/>
<input_port name="canlist"
type="std::string"/>
</Action>
<Action ID="I2CSignal">
<input_port name="Address"
default="42"
@ -58,18 +335,109 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<Action ID="LookAtNode">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveGlobal">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="6.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="4.000000"
type="double"/>
<input_port name="ornt_mult"
default="3.000000"
type="double"/>
<input_port name="tolerance"
default="0.050000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveLocal">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="2.000000"
type="double"/>
<input_port name="pos_mult"
default="15.000000"
type="double"/>
<input_port name="ornt_mult"
default="4.000000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="2.000000"
type="double"/>
<input_port name="ornt_mult"
default="4.000000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateNode">
<input_port name="angle"
type="double"/>
<input_port name="isDegree"
default="true"
type="bool"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
@ -80,7 +448,7 @@
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue"
type="int"/>
type="bool"/>
<output_port name="tactic"
type="int"/>
<input_port name="number"