Skip to content

Commit 503a430

Browse files
author
Bilal Gill
committed
Update to make sure poses are ordered in path
1 parent 7dc5a96 commit 503a430

File tree

2 files changed

+241
-3
lines changed

2 files changed

+241
-3
lines changed

‎src/hangar_sim/objectives/count_boxes.xml‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,11 +33,20 @@
3333
poses_cropped="{path_cropped}"
3434
crop_box_size="12;12;1"
3535
/>
36+
<SubTree
37+
ID="Order Poses"
38+
_collapsed="true"
39+
centroid="{crop_pose_centroid}"
40+
poses_in="{path_cropped}"
41+
poses_ordered="{poses_ordered}"
42+
x_extent="12"
43+
y_extent="12"
44+
/>
3645
<Action
3746
ID="VisualizePath"
3847
marker_lifetime="0.000000"
39-
marker_name="path_cropped"
40-
path="{path_cropped}"
48+
marker_name="poses_ordered"
49+
path="{poses_ordered}"
4150
pose_marker_size="1"
4251
show_poses="true"
4352
/>
@@ -57,7 +66,7 @@
5766
ID="ForEach"
5867
index="{index}"
5968
out="{out}"
60-
vector_in="{path_cropped}"
69+
vector_in="{poses_ordered}"
6170
>
6271
<Control ID="Sequence">
6372
<Action

‎src/hangar_sim/objectives/order_poses.xml‎

Lines changed: 229 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,229 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Order Poses">
3+
<BehaviorTree
4+
ID="Order Poses"
5+
_description="Orders poses for the Count Boxes Objective"
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<Action ID="CreateFloat64Message" data="{x_extent}" float64="{x_msg}" />
10+
<Action ID="UnpackFloat64Message" data="{x_extent}" float64="{x_msg}" />
11+
<Action ID="CreateFloat64Message" data="{y_extent}" float64="{y_msg}" />
12+
<Action ID="UnpackFloat64Message" data="{y_extent}" float64="{y_msg}" />
13+
<Action ID="Script" code="poses_ordered:=poses_in" />
14+
<Control ID="Sequence">
15+
<Action
16+
ID="UnpackPoseStampedMessage"
17+
header="{header}"
18+
pose="{pose}"
19+
pose_stamped="{centroid}"
20+
/>
21+
<Action
22+
ID="UnpackPoseMessage"
23+
orientation="{orientation}"
24+
pose="{pose}"
25+
position="{position}"
26+
/>
27+
<Action
28+
ID="UnpackPointMessage"
29+
point="{position}"
30+
x="{x}"
31+
y="{y}"
32+
z="{z}"
33+
/>
34+
<Action ID="Script" code="left:=x-(x_extent/2)" />
35+
<Action ID="Script" code="right:=x+(x_extent/2)" />
36+
<Action ID="Script" code="top:=y+(y_extent/2)" />
37+
<Action ID="Script" code="bottom:=y-(y_extent/2)" />
38+
<Control ID="Sequence">
39+
<Action
40+
ID="CreatePointMessage"
41+
point="{bottom_left}"
42+
x="{left}"
43+
y="{bottom}"
44+
z="{z}"
45+
/>
46+
<Action
47+
ID="CreatePoseMessage"
48+
orientation="{orientation}"
49+
pose="{pose_bottom_left}"
50+
position="{bottom_left}"
51+
/>
52+
<Action
53+
ID="CreatePoseStampedMessage"
54+
header="{header}"
55+
pose="{pose_bottom_left}"
56+
pose_stamped="{pose_stamped_bottom_left}"
57+
/>
58+
</Control>
59+
<Control ID="Sequence">
60+
<Action
61+
ID="CreatePointMessage"
62+
point="{top_left}"
63+
x="{left}"
64+
y="{top}"
65+
z="{z}"
66+
/>
67+
<Action
68+
ID="CreatePoseMessage"
69+
orientation="{orientation}"
70+
pose="{pose_top_left}"
71+
position="{top_left}"
72+
/>
73+
<Action
74+
ID="CreatePoseStampedMessage"
75+
header="{header}"
76+
pose="{pose_top_left}"
77+
pose_stamped="{pose_stamped_top_left}"
78+
/>
79+
</Control>
80+
<Control ID="Sequence">
81+
<Action
82+
ID="CreatePointMessage"
83+
point="{top_right}"
84+
x="{right}"
85+
y="{top}"
86+
z="{z}"
87+
/>
88+
<Action
89+
ID="CreatePoseMessage"
90+
orientation="{orientation}"
91+
pose="{pose_top_right}"
92+
position="{top_right}"
93+
/>
94+
<Action
95+
ID="CreatePoseStampedMessage"
96+
header="{header}"
97+
pose="{pose_top_right}"
98+
pose_stamped="{pose_stamped_top_right}"
99+
/>
100+
</Control>
101+
<Control ID="Sequence">
102+
<Action
103+
ID="CreatePointMessage"
104+
point="{bottom_right}"
105+
x="{right}"
106+
y="{bottom}"
107+
z="{z}"
108+
/>
109+
<Action
110+
ID="CreatePoseMessage"
111+
orientation="{orientation}"
112+
pose="{pose_bottom_right}"
113+
position="{bottom_right}"
114+
/>
115+
<Action
116+
ID="CreatePoseStampedMessage"
117+
header="{header}"
118+
pose="{pose_bottom_right}"
119+
pose_stamped="{pose_stamped_bottom_right}"
120+
/>
121+
</Control>
122+
</Control>
123+
<Control ID="Sequence">
124+
<Decorator
125+
ID="ForEach"
126+
index="{index}"
127+
out="{out}"
128+
vector_in="{poses_in}"
129+
>
130+
<Control ID="Fallback">
131+
<Decorator ID="Precondition" else="FAILURE" if="index == 0">
132+
<Control ID="Sequence">
133+
<SubTree
134+
ID="Find Nearest Pose In Path"
135+
_collapsed="true"
136+
nearest_index="{nearest_index}"
137+
nearest_pose="{nearest_pose}"
138+
pose_vector="{poses_in}"
139+
search_pose="{pose_stamped_bottom_left}"
140+
/>
141+
<Action
142+
ID="ReplaceInVector"
143+
index="{index}"
144+
input_vector="{poses_ordered}"
145+
output_vector="{poses_ordered}"
146+
element="{nearest_pose}"
147+
/>
148+
</Control>
149+
</Decorator>
150+
<Decorator ID="Precondition" else="FAILURE" if="index == 1">
151+
<Control ID="Sequence">
152+
<SubTree
153+
ID="Find Nearest Pose In Path"
154+
nearest_index="{nearest_index}"
155+
nearest_pose="{nearest_pose}"
156+
pose_vector="{poses_in}"
157+
search_pose="{pose_stamped_top_left}"
158+
/>
159+
<Action
160+
ID="ReplaceInVector"
161+
index="{index}"
162+
input_vector="{poses_ordered}"
163+
output_vector="{poses_ordered}"
164+
element="{nearest_pose}"
165+
/>
166+
</Control>
167+
</Decorator>
168+
<Decorator ID="Precondition" else="FAILURE" if="index == 2">
169+
<Control ID="Sequence">
170+
<SubTree
171+
ID="Find Nearest Pose In Path"
172+
nearest_index="{nearest_index}"
173+
nearest_pose="{nearest_pose}"
174+
pose_vector="{poses_in}"
175+
search_pose="{pose_stamped_top_right}"
176+
/>
177+
<Action
178+
ID="ReplaceInVector"
179+
index="{index}"
180+
input_vector="{poses_ordered}"
181+
output_vector="{poses_ordered}"
182+
element="{nearest_pose}"
183+
/>
184+
</Control>
185+
</Decorator>
186+
<Decorator ID="Precondition" else="FAILURE" if="index == 3">
187+
<Control ID="Sequence">
188+
<SubTree
189+
ID="Find Nearest Pose In Path"
190+
nearest_index="{nearest_index}"
191+
nearest_pose="{nearest_pose}"
192+
pose_vector="{poses_in}"
193+
search_pose="{pose_stamped_bottom_right}"
194+
/>
195+
<Action
196+
ID="ReplaceInVector"
197+
index="{index}"
198+
input_vector="{poses_ordered}"
199+
output_vector="{poses_ordered}"
200+
element="{nearest_pose}"
201+
/>
202+
</Control>
203+
</Decorator>
204+
<Decorator ID="ForceFailure">
205+
<Action
206+
ID="LogMessage"
207+
log_level="error"
208+
message="This shouldn't get over here"
209+
/>
210+
</Decorator>
211+
</Control>
212+
</Decorator>
213+
</Control>
214+
</Control>
215+
</BehaviorTree>
216+
<TreeNodesModel>
217+
<SubTree ID="Order Poses">
218+
<inout_port
219+
name="centroid"
220+
default="{centroid}"
221+
type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
222+
/>
223+
<inout_port name="poses_in" default="{poses_in}" />
224+
<inout_port name="poses_ordered" default="{poses_ordered}" />
225+
<inout_port name="x_extent" default="{x_extent}" />
226+
<inout_port name="y_extent" default="{y_extent}" />
227+
</SubTree>
228+
</TreeNodesModel>
229+
</root>

0 commit comments

Comments
 (0)