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

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +49 -49
5 changed files with 49 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d26adb576a - Show all commits

2
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -73,7 +73,7 @@
<joint name="lidar_joint" type="fixed"> <joint name="lidar_joint" type="fixed">
<parent link="base_link"/> <parent link="base_link"/>
<child link="lidar"/> <child link="lidar"/>
<origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/> <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 ${pi}"/>
</joint> </joint>

54
toid_bt/behavior_trees/behavior1.xml
View File

@@ -15,39 +15,35 @@
<BehaviorTree ID="seq1"> <BehaviorTree ID="seq1">
<Sequence> <Sequence>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<MovePointSimple x="0.1" <SetInitialPose y="0.805"
y="0" theta="-90"
theta="0" frame_id="map"
max_speed="0.350000" x="1.325"
topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/>
<InPose timeout="1.000000">
<MovePointSimple x="1.325"
y="0.200"
theta="-90"
max_speed="0.200000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<RotateTowards x="0.76" </InPose>
y="-0.5" <Seq1 service_name=""/>
max_speed="1.50000" <Seq2 text=""
min_angle="0.000000" service_name=""/>
action_name=""/> <MovePointSimple x="1.325"
<MovePointSimple x="0.756" y="-0.05"
y="-0.5" theta="-90"
theta="-45" max_speed="0.200000"
max_speed="0.350000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<RotateTowards x="0.76" <Seq3 service_name=""/>
y="-0.02" <MovePointSimple x="1.325"
max_speed="1.50000" y="0.805"
min_angle="0.000000" theta="-90"
action_name=""/> max_speed="0.200000"
<MovePointSimple x="0.757" backwards="true"
y="-0.02"
theta="90"
max_speed="0.150000"
backwards="false"
action_name=""/>
<MovePointSimple x="1.2"
y="0.0"
theta="0.0"
max_speed="0.250000"
backwards="false"
action_name=""/> action_name=""/>
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>

21
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -4,6 +4,7 @@
<Sequence> <Sequence>
<SetBlackboard value="0.265" <SetBlackboard value="0.265"
output_key="width"/> output_key="width"/>
<WaitPullPin />
<Sleep msec="1000"/> <Sleep msec="1000"/>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<RotateTowards x="0.4" <RotateTowards x="0.4"
@@ -46,52 +47,53 @@
<Sequence> <Sequence>
<Sleep msec="5000"/> <Sleep msec="5000"/>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<WaitPullPin />
<Sleep msec="1000"/> <Sleep msec="1000"/>
<Repeat num_cycles="2"> <Repeat num_cycles="5">
<Sequence> <Sequence>
<MovePointSimple x="1.1" <MovePointSimple x="1.1"
y="0" y="0"
theta="0" theta="0"
max_speed="0.10000" max_speed="0.30000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<Sleep msec="1000"/> <Sleep msec="1000"/>
<RotateSimple angle="180" <RotateSimple angle="180"
max_speed="0.300000" max_speed="1.300000"
min_angle="10" min_angle="10"
action_name=""/> action_name=""/>
<Sleep msec="500"/> <Sleep msec="500"/>
<MovePointSimple x="0.35" <MovePointSimple x="0.35"
y="0" y="0"
theta="180" theta="180"
max_speed="0.100000" max_speed="0.300000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<RotateSimple angle="0" <RotateSimple angle="0"
max_speed="0.300000" max_speed="1.300000"
min_angle="-10" min_angle="-10"
action_name=""/> action_name=""/>
<Sleep msec="500"/> <Sleep msec="500"/>
<MovePointSimple x="1.1" <MovePointSimple x="1.1"
y="0" y="0"
theta="0" theta="0"
max_speed="0.10000" max_speed="0.30000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<Sleep msec="500"/> <Sleep msec="500"/>
<RotateSimple angle="180" <RotateSimple angle="180"
max_speed="0.300000" max_speed="1.300000"
min_angle="-10" min_angle="-10"
action_name=""/> action_name=""/>
<Sleep msec="500"/> <Sleep msec="500"/>
<MovePointSimple x="0.35" <MovePointSimple x="0.35"
y="0" y="0"
theta="180" theta="180"
max_speed="0.100000" max_speed="0.300000"
backwards="false" backwards="false"
action_name=""/> action_name=""/>
<RotateSimple angle="0" <RotateSimple angle="0"
max_speed="0.300000" max_speed="1.300000"
min_angle="10" min_angle="10"
action_name=""/> action_name=""/>
</Sequence> </Sequence>
@@ -113,6 +115,7 @@
<BehaviorTree ID="wheel_size"> <BehaviorTree ID="wheel_size">
<Sequence> <Sequence>
<WaitPullPin />
<Sleep msec="1000"/> <Sleep msec="1000"/>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<Sleep msec="1000"/> <Sleep msec="1000"/>

10
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -59,11 +59,11 @@
<input_port name="service_name" type="std::string">Service name</input_port> <input_port name="service_name" type="std::string">Service name</input_port>
</Action> </Action>
<Condition ID="SetInitialPose"> <Condition ID="SetInitialPose">
<input_port name="y" type="double" default="0.000000">Y position in meters</input_port> <input_port name="y" default="0.000000" type="double">Y position in meters</input_port>
<input_port name="theta" type="double" default="0.000000">Heading in degrees</input_port> <input_port name="theta" default="0.000000" type="double">Heading in degrees</input_port>
<input_port name="frame_id" type="std::string" default="map">Reference frame</input_port> <input_port name="frame_id" default="map" type="std::string">Reference frame</input_port>
<input_port name="x" type="double" default="0.000000">X position in meters</input_port> <input_port name="x" default="0.000000" type="double">X position in meters</input_port>
<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> </Condition>
<Action ID="SetParameter"> <Action ID="SetParameter">
<input_port name="parameter" type="std::string"/> <input_port name="parameter" type="std::string"/>

3
toid_navigation/params/toid_general_params.yaml
View File

@@ -29,12 +29,13 @@ behavior_server:
plugin: "nav2_behaviors::BackUp" plugin: "nav2_behaviors::BackUp"
rotate: rotate:
plugin: "toid::SimpleRotateBehavior" plugin: "toid::SimpleRotateBehavior"
max_angular_accel: 4.0 max_angular_accel: 3.0
max_angular_decel: 1.0 max_angular_decel: 1.0
translateX: translateX:
plugin: "toid::SimpleTranslateXBehavior" plugin: "toid::SimpleTranslateXBehavior"
moveCoords: moveCoords:
plugin: "toid::MoveCoords" plugin: "toid::MoveCoords"
max_vel_accel: 1.0
approachAcorns: approachAcorns:
plugin: "toid::ApproachAcorns" plugin: "toid::ApproachAcorns"
kphi: 1.2 kphi: 1.2