Skip to content

Commit 55605ac

Browse files
davetcolemandavetcoleman
committed
Objectathon Example Vector and String Handling
1 parent 5f1988e commit 55605ac

File tree

6 files changed

+192
-4
lines changed

6 files changed

+192
-4
lines changed

‎src/lab_sim/objectives/errorcheckvectorsize.xml‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="ErrorCheckVectorSize">
3+
<!--//////////-->
4+
<BehaviorTree ID="ErrorCheckVectorSize">
5+
<Control ID="Sequence">
6+
<Action
7+
ID="GetSizeOfVector"
8+
vector_in="{vector}"
9+
vector_size="{vector_size}"
10+
/>
11+
<Decorator
12+
ID="Precondition"
13+
else="SUCCESS"
14+
if="vector_size!=expected_vector_size"
15+
name="Error Check "
16+
>
17+
<Control ID="Sequence">
18+
<Action
19+
ID="LogMessage"
20+
log_level="error"
21+
message="'Vector size is wrong'"
22+
/>
23+
<Action ID="AlwaysFailure" />
24+
</Control>
25+
</Decorator>
26+
</Control>
27+
</BehaviorTree>
28+
<TreeNodesModel>
29+
<SubTree ID="ErrorCheckVectorSize">
30+
<MetadataFields>
31+
<Metadata runnable="false" />
32+
<Metadata subcategory="Error Checking" />
33+
</MetadataFields>
34+
<inout_port name="expected_vector_size" default="1" type="int">
35+
The size the vector should be
36+
</inout_port>
37+
<inout_port name="vector" default="{vector}">
38+
The input vector that is to be checked for size.
39+
</inout_port>
40+
</SubTree>
41+
</TreeNodesModel>
42+
</root>

‎src/lab_sim/objectives/example_file.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
1) A robot may not injure a human being or, through inaction, allow a human being to come to harm;
2+
2) A robot must obey orders from a human, except when they conflict with the First Law; and
3+
3) A robot must protect its own existence, provided it doesn't conflict with the First or Second Law.

‎src/lab_sim/objectives/pick_1_pill_bottle.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,7 @@
7474
controller_names="joint_trajectory_controller;robotiq_gripper_controller"
7575
task="{mtc_task}"
7676
trajectory_monitoring="false"
77+
timeout="-1.000000"
7778
/>
7879
<Action ID="SetupMTCCurrentState" task="{mtc_task}" />
7980
<Action

‎src/lab_sim/objectives/simple_test.xml‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Simple Test">
3+
<!--//////////-->
4+
<BehaviorTree ID="Simple Test" _description="" _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<!--Reset vector to ensure it's empty-->
7+
<Action ID="ResetVector" vector="{my_vector}" />
8+
<!--Create first pose and insert at index 0-->
9+
<Action
10+
ID="CreateStampedPose"
11+
position_xyz="0.5;0.0;0.5"
12+
orientation_xyzw="0;0;0;1"
13+
reference_frame="world"
14+
stamped_pose="{pose1}"
15+
/>
16+
<Action
17+
ID="InsertInVector"
18+
element="{pose1}"
19+
index="0"
20+
input_vector="{my_vector}"
21+
output_vector="{my_vector}"
22+
/>
23+
<!--Create second pose and insert at index 1-->
24+
<Action
25+
ID="CreateStampedPose"
26+
position_xyz="0.6;0.1;0.5"
27+
orientation_xyzw="0;0;0;1"
28+
reference_frame="world"
29+
stamped_pose="{pose2}"
30+
/>
31+
<Action
32+
ID="InsertInVector"
33+
element="{pose2}"
34+
index="1"
35+
input_vector="{my_vector}"
36+
output_vector="{my_vector}"
37+
/>
38+
</Control>
39+
</BehaviorTree>
40+
<TreeNodesModel>
41+
<SubTree ID="Simple Test">
42+
<MetadataFields>
43+
<Metadata runnable="true" />
44+
<Metadata subcategory="_Dave Tests" />
45+
</MetadataFields>
46+
</SubTree>
47+
</TreeNodesModel>
48+
</root>

‎src/lab_sim/objectives/string_and_vector_example.xml‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="String and Vector Example">
3+
<BehaviorTree
4+
ID="String and Vector Example"
5+
_description=""
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<Action ID="ResetVector" vector="{my_vector}" name="Create the Vector" />
10+
<Action
11+
ID="ReadTextFileAsString"
12+
string_file_contents="{string1}"
13+
text_filename="example_file.txt"
14+
/>
15+
<Action
16+
ID="PublishString"
17+
queue_size="1"
18+
use_best_effort="false"
19+
message="{string1}"
20+
topic="/ai_safety_announcement"
21+
/>
22+
<Action ID="LogMessage" message="{string1}" log_level="warn" />
23+
<Control ID="Sequence" name="Test Some Vector Operations">
24+
<Action
25+
ID="InsertInVector"
26+
element="{json:{&quot;__type&quot;:&quot;string&quot;,&quot;value&quot;:&quot;{string1}&quot;}"
27+
index="0"
28+
input_vector="{my_vector}"
29+
output_vector="{my_vector}"
30+
/>
31+
<SubTree
32+
ID="ErrorCheckVectorSize"
33+
_collapsed="true"
34+
expected_vector_size="1"
35+
vector="{my_vector}"
36+
/>
37+
<Action
38+
ID="RemoveFromVector"
39+
index="0"
40+
input_vector="{my_vector}"
41+
output_vector="{my_vector}"
42+
/>
43+
<SubTree
44+
ID="ErrorCheckVectorSize"
45+
_collapsed="true"
46+
expected_vector_size="0"
47+
vector="{my_vector}"
48+
/>
49+
</Control>
50+
<Control ID="Sequence" name="Item 1">
51+
<Action ID="Script" code="string2 := 'Do not be evil'" />
52+
<Action
53+
ID="InsertInVector"
54+
element="{json:{&quot;__type&quot;:&quot;string&quot;,&quot;value&quot;:&quot;{string2}&quot;}"
55+
index="0"
56+
input_vector="{my_vector}"
57+
output_vector="{my_vector}"
58+
/>
59+
<SubTree
60+
ID="ErrorCheckVectorSize"
61+
_collapsed="true"
62+
expected_vector_size="1"
63+
vector="{my_vector}"
64+
/>
65+
<Action ID="LogMessage" message="{string2}" log_level="warn" />
66+
<Action ID="Script" code="string3 := 'robots'" />
67+
<Action
68+
ID="ReplaceInVector"
69+
element="{json:{&quot;__type&quot;:&quot;string&quot;,&quot;value&quot;:&quot;{string3}&quot;}"
70+
index="0"
71+
input_vector="{my_vector}"
72+
output_vector="{my_vector}"
73+
/>
74+
<SubTree
75+
ID="ErrorCheckVectorSize"
76+
_collapsed="true"
77+
expected_vector_size="1"
78+
vector="{my_vector}"
79+
/>
80+
<Action ID="LogMessage" message="{string3}" log_level="warn" />
81+
</Control>
82+
</Control>
83+
</BehaviorTree>
84+
<TreeNodesModel>
85+
<SubTree ID="String and Vector Example">
86+
<MetadataFields>
87+
<Metadata runnable="true" />
88+
<Metadata subcategory="_Dave Tests" />
89+
</MetadataFields>
90+
</SubTree>
91+
</TreeNodesModel>
92+
</root>

‎src/lab_sim/test/objectives_integration_test.py‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
run_objective,
3838
)
3939

40+
# Looping objectives to cancel partway through
4041
cancel_objectives = {
4142
"3 Waypoints Pick and Place",
4243
"Cycle Between Waypoints",
@@ -54,17 +55,18 @@
5455
"ML Grasp Object from Text Prompt",
5556
}
5657

58+
# Objectives to skip entirely from integration testing
5759
skip_objectives = {
5860
"Grasp Object from Text Prompt", # https://github.com/PickNikRobotics/moveit_pro/issues/13236
5961
"Grasp Planning",
6062
"Joint Diagnostic",
61-
"Fuse Multiple Views",
63+
"Scan Multiple Views",
6264
"MPC Pose Tracking",
6365
"MPC Pose Tracking With Point Cloud Avoidance",
64-
"ML Segment Image from Text Prompt",
66+
"ML Segment Image",
6567
"ML Segment Point Cloud from Clicked Point",
66-
"ML Auto Grasp Object from Clicked Point", # Skipped because there is no primary ui to switch to in ci
67-
"ML Auto Grasp Object from Text Prompt",
68+
"Pick All Pill Bottles", # Skipped because there is no primary ui to switch to in ci
69+
"Pick 1 Pill Bottle",
6870
"Pick up Cube",
6971
"Place Object",
7072
"Record Square Trajectory",

0 commit comments

Comments
 (0)