Skip to content

Commit d07530c

Browse files
rlpratt12rlpratt12
authored
Merge pull request #417 from PickNikRobotics/objectathon2
Objectathon Example Vector and String Handling
2 parents 11f4499 + 16d9098 commit d07530c

File tree

3 files changed

+197
-0
lines changed

3 files changed

+197
-0
lines changed

‎src/lab_sim/objectives/errorcheckvectorsize.xml‎

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

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Vector and String Example">
3+
<BehaviorTree
4+
ID="Vector and String Example"
5+
_description="Showcases creating, inserting, replacing, and removing strings from a vector, as well as reading strings from file and publishing them to ROS."
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<Action ID="ResetVector" vector="{my_vector}" name="Create the Vector" />
10+
<Control ID="Sequence" name="Test Some Vector Operations">
11+
<Action
12+
ID="Script"
13+
code="string1 := 'Copy and run the following command in your &quot;moveit_pro shell&quot; terminal, then rerun this Objective to get the secret message.'"
14+
/>
15+
<Action
16+
ID="CreateStringMessage"
17+
data="{string1}"
18+
string="{string_msg_in}"
19+
/>
20+
<Action
21+
ID="InsertInVector"
22+
element="{string_msg_in}"
23+
index="0"
24+
input_vector="{my_vector}"
25+
output_vector="{my_vector}"
26+
/>
27+
<SubTree
28+
ID="ErrorCheckVectorSize"
29+
_collapsed="true"
30+
expected_vector_size="1"
31+
vector="{my_vector}"
32+
/>
33+
<Action
34+
ID="GetElementOfVector"
35+
element="{string_msg_out}"
36+
index="0"
37+
vector_in="{my_vector}"
38+
/>
39+
<Action
40+
ID="UnpackStringMessage"
41+
data="{string}"
42+
string="{string_msg_out}"
43+
/>
44+
<Action
45+
ID="LogMessage"
46+
message="ros2 topic echo /secret_msg std_msgs/msg/String --qos-profile system_default"
47+
log_level="info"
48+
/>
49+
<Action ID="LogMessage" message="{string}" log_level="info" />
50+
</Control>
51+
<Control ID="Sequence" name="Test Some Vector Operations">
52+
<Action
53+
ID="RemoveFromVector"
54+
index="0"
55+
input_vector="{my_vector}"
56+
output_vector="{my_vector}"
57+
/>
58+
<SubTree
59+
ID="ErrorCheckVectorSize"
60+
_collapsed="true"
61+
expected_vector_size="0"
62+
vector="{my_vector}"
63+
/>
64+
</Control>
65+
<Control ID="Sequence" name="Test Some Vector Operations">
66+
<Action
67+
ID="ReadTextFileAsString"
68+
string_file_contents="{string2}"
69+
text_filename="example_file.txt"
70+
/>
71+
<Action
72+
ID="CreateStringMessage"
73+
data="{string2}"
74+
string="{string_msg_in}"
75+
/>
76+
<Action
77+
ID="InsertInVector"
78+
element="{string_msg_in}"
79+
index="0"
80+
input_vector="{my_vector}"
81+
output_vector="{my_vector}"
82+
/>
83+
<Action
84+
ID="GetElementOfVector"
85+
element="{string_msg_out}"
86+
index="0"
87+
vector_in="{my_vector}"
88+
/>
89+
<Action
90+
ID="PublishString"
91+
queue_size="10"
92+
use_best_effort="false"
93+
message="{string_msg_out}"
94+
topic="/secret_msg"
95+
/>
96+
<SubTree
97+
ID="ErrorCheckVectorSize"
98+
_collapsed="true"
99+
expected_vector_size="1"
100+
vector="{my_vector}"
101+
/>
102+
</Control>
103+
<Control ID="Sequence" name="Test Some Vector Operations">
104+
<Action
105+
ID="Script"
106+
code="string3 := 'Make sure you have INFO alert types enables in order to see the messages.'"
107+
/>
108+
<Action
109+
ID="CreateStringMessage"
110+
data="{string3}"
111+
string="{string_msg_in}"
112+
/>
113+
<Action
114+
ID="ReplaceInVector"
115+
element="{string_msg_in}"
116+
index="0"
117+
input_vector="{my_vector}"
118+
output_vector="{my_vector}"
119+
/>
120+
<Action
121+
ID="GetElementOfVector"
122+
element="{string_msg_out}"
123+
index="0"
124+
vector_in="{my_vector}"
125+
/>
126+
<Action
127+
ID="UnpackStringMessage"
128+
data="{string}"
129+
string="{string_msg_out}"
130+
/>
131+
<SubTree
132+
ID="ErrorCheckVectorSize"
133+
_collapsed="true"
134+
expected_vector_size="1"
135+
vector="{my_vector}"
136+
/>
137+
<Action ID="LogMessage" message="{string}" log_level="warn" />
138+
</Control>
139+
</Control>
140+
</BehaviorTree>
141+
<TreeNodesModel>
142+
<SubTree ID="Vector and String Example">
143+
<MetadataFields>
144+
<Metadata subcategory="Vector Handling" />
145+
<Metadata runnable="true" />
146+
</MetadataFields>
147+
</SubTree>
148+
</TreeNodesModel>
149+
</root>

0 commit comments

Comments
 (0)