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

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"