Skip to content

Commit d011fc9

Browse files
MikeWrockMikeWrock
committed
Added logging, more subtrees, file path warnings, and port remaps
1 parent c743719 commit d011fc9

File tree

7 files changed

+274
-151
lines changed

7 files changed

+274
-151
lines changed

‎src/hangar_sim/objectives/count_boxes.xml‎

Lines changed: 9 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -1,96 +1,25 @@
11
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Count Boxes">
3+
<!--//////////-->
34
<BehaviorTree
45
ID="Count Boxes"
56
_description="Count the number of boxes in a given area."
67
_favorite="false"
8+
name="Show images taken by the robot to a human for box count verification"
79
>
810
<Control ID="Sequence" name="TopLevelSequence">
9-
<SubTree
10-
ID="CreatePoseVectorFromDirectory"
11-
_collapsed="true"
12-
pose_vector="{pose_vector}"
13-
directory_path="/home/studio-user/user_ws/src/hangar_sim/poses"
14-
/>
1511
<Action
16-
ID="CreateStampedPose"
17-
reference_frame="world"
18-
stamped_pose="{crop_pose_centroid}"
19-
position_xyz="-11;2.75;0"
20-
/>
21-
<Action
22-
ID="VisualizePath"
23-
marker_lifetime="0.000000"
24-
marker_name="path_initial"
25-
path="{pose_vector}"
26-
pose_marker_size="1"
27-
show_poses="true"
28-
/>
29-
<Action
30-
ID="CropPosesInBox"
31-
crop_box_centroid_pose="{crop_pose_centroid}"
32-
poses="{pose_vector}"
33-
poses_cropped="{path_cropped}"
34-
crop_box_size="12;12;1"
12+
ID="LogMessage"
13+
log_level="error"
14+
message="Make sure to update the absolute path by replacing studio-user with your username, then you can delete this message"
3515
/>
3616
<SubTree
37-
ID="Order Poses"
17+
ID="Count Boxes Objective"
3818
_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-
/>
45-
<Action
46-
ID="VisualizePath"
47-
marker_lifetime="0.000000"
48-
marker_name="poses_ordered"
49-
path="{poses_ordered}"
50-
pose_marker_size="1"
51-
show_poses="true"
52-
/>
53-
<Action
54-
ID="SwitchController"
55-
activate_asap="true"
56-
automatic_deactivation="true"
57-
controller_list_action_name="/controller_manager/list_controllers"
58-
controller_switch_action_name="/controller_manager/switch_controller"
59-
strictness="1"
60-
timeout="-1.000000"
61-
activate_controllers="platform_velocity_controller_nav2"
62-
deactivate_controllers="platform_velocity_controller"
19+
pose_path="/home/studio-user/user_ws/src/hangar_sim/poses"
20+
scene_camera_folder="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
21+
wrist_camera_folder="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
6322
/>
64-
<Action ID="Script" code="total_box_count:=0" />
65-
<Decorator
66-
ID="ForEach"
67-
index="{index}"
68-
out="{out}"
69-
vector_in="{poses_ordered}"
70-
>
71-
<Control ID="Sequence">
72-
<Action
73-
ID="NavigateToPoseAction"
74-
action_name="/navigate_to_pose"
75-
current_pose="{current_pose}"
76-
distance_remaining="{distance_remaining}"
77-
estimated_time_remaining="{estimated_time_remaining}"
78-
ignore_stamp_time="true"
79-
navigation_time="{navigation_time}"
80-
number_of_recoveries="{number_of_recoveries}"
81-
pose_stamped="{out}"
82-
timeout_sec="-1.000000"
83-
behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
84-
/>
85-
<SubTree
86-
ID="Count boxes subtree"
87-
_collapsed="false"
88-
box_count="{box_count}"
89-
/>
90-
<Action ID="Script" code="total_box_count+=box_count" />
91-
</Control>
92-
</Decorator>
93-
<Action ID="LogMessage" log_level="info" message="{total_box_count}" />
9423
</Control>
9524
</BehaviorTree>
9625
<TreeNodesModel>

‎src/hangar_sim/objectives/count_boxes_objective.xml‎

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Count Boxes Objective">
3+
<BehaviorTree
4+
ID="Count Boxes Objective"
5+
_description="Count the number of boxes in a given area."
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<SubTree
10+
ID="CreatePoseVectorFromDirectory"
11+
_collapsed="false"
12+
pose_vector="{pose_vector}"
13+
directory_path="{pose_path}"
14+
name="given a path, load a PoseStamped from each yaml in that directory"
15+
/>
16+
<!--Crop outliers from our region of interest-->
17+
<Action
18+
ID="CreateStampedPose"
19+
reference_frame="world"
20+
stamped_pose="{crop_pose_centroid}"
21+
position_xyz="-11;2.75;0"
22+
/>
23+
<Action
24+
ID="VisualizePath"
25+
marker_lifetime="0.000000"
26+
marker_name="path_initial"
27+
path="{pose_vector}"
28+
pose_marker_size="1"
29+
show_poses="true"
30+
/>
31+
<Action
32+
ID="CropPosesInBox"
33+
crop_box_centroid_pose="{crop_pose_centroid}"
34+
poses="{pose_vector}"
35+
poses_cropped="{path_cropped}"
36+
crop_box_size="12;12;1"
37+
/>
38+
<SubTree
39+
ID="Order Poses"
40+
_collapsed="false"
41+
centroid="{crop_pose_centroid}"
42+
poses_in="{path_cropped}"
43+
poses_ordered="{poses_ordered}"
44+
x_extent="12"
45+
y_extent="12"
46+
/>
47+
<Action
48+
ID="VisualizePath"
49+
marker_lifetime="0.000000"
50+
marker_name="poses_ordered"
51+
path="{poses_ordered}"
52+
pose_marker_size="1"
53+
show_poses="true"
54+
/>
55+
<Action
56+
ID="SwitchController"
57+
activate_asap="true"
58+
automatic_deactivation="true"
59+
controller_list_action_name="/controller_manager/list_controllers"
60+
controller_switch_action_name="/controller_manager/switch_controller"
61+
strictness="1"
62+
timeout="-1.000000"
63+
activate_controllers="platform_velocity_controller_nav2"
64+
deactivate_controllers="platform_velocity_controller"
65+
/>
66+
<Action ID="Script" code="total_box_count:=0" />
67+
<!--Drive through our newly ordered list of poses and count boxes at each Pose-->
68+
<Decorator
69+
ID="ForEach"
70+
index="{index}"
71+
out="{out}"
72+
vector_in="{poses_ordered}"
73+
>
74+
<Control ID="Sequence">
75+
<Action
76+
ID="NavigateToPoseAction"
77+
action_name="/navigate_to_pose"
78+
current_pose="{current_pose}"
79+
distance_remaining="{distance_remaining}"
80+
estimated_time_remaining="{estimated_time_remaining}"
81+
ignore_stamp_time="true"
82+
navigation_time="{navigation_time}"
83+
number_of_recoveries="{number_of_recoveries}"
84+
pose_stamped="{out}"
85+
timeout_sec="-1.000000"
86+
behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
87+
/>
88+
<SubTree
89+
ID="Count boxes subtree"
90+
_collapsed="false"
91+
box_count="{box_count}"
92+
scene_camera_folder="{scene_camera_folder}"
93+
wrist_camera_folder="{wrist_camera_folder}"
94+
/>
95+
<Action ID="Script" code="total_box_count+=box_count" />
96+
</Control>
97+
</Decorator>
98+
<Action ID="LogMessage" log_level="info" message="{total_box_count}" />
99+
</Control>
100+
</BehaviorTree>
101+
<TreeNodesModel>
102+
<SubTree ID="Count Boxes Objective">
103+
<MetadataFields>
104+
<Metadata runnable="false" />
105+
<Metadata subcategory="Application - Advanced Examples" />
106+
</MetadataFields>
107+
<inout_port
108+
name="wrist_camera_folder"
109+
default="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
110+
type="std::string"
111+
>
112+
Absolute path to a directory for saving wrist camera pictures
113+
</inout_port>
114+
<inout_port
115+
name="scene_camera_folder"
116+
default="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
117+
type="std::string"
118+
>
119+
Absolute path to a directory for saving scene camera pictures
120+
</inout_port>
121+
<inout_port
122+
name="pose_path"
123+
default="/home/studio-user/user_ws/src/hangar_sim/poses"
124+
type="std::string"
125+
>
126+
Absolute path to a directory of pose YAML files
127+
</inout_port>
128+
</SubTree>
129+
</TreeNodesModel>
130+
</root>

‎src/hangar_sim/objectives/count_boxes_subtree.xml‎

Lines changed: 19 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,13 @@
2020
/>
2121
<Action
2222
ID="SaveImageToFile"
23-
file_path="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
23+
file_path="{wrist_camera_folder}"
2424
file_prefix="box"
2525
image="{wrist_camera_image}"
2626
/>
2727
<Action
2828
ID="SaveImageToFile"
29-
file_path="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
29+
file_path="{scene_camera_folder}"
3030
file_prefix="box"
3131
image="{scene_camera_image}"
3232
/>
@@ -142,13 +142,27 @@
142142
</BehaviorTree>
143143
<TreeNodesModel>
144144
<SubTree ID="Count boxes subtree">
145-
<inout_port name="box_count" default="{box_count}" type="int">
146-
Number of boxes
147-
</inout_port>
148145
<MetadataFields>
149146
<Metadata runnable="False" />
150147
<Metadata subcategory="Perception - ML" />
151148
</MetadataFields>
149+
<inout_port
150+
name="scene_camera_folder"
151+
default="/home/studio-user/user_ws/src/hangar_sim/scene_camera_images"
152+
type="std::string"
153+
>
154+
Absolute path to a directory for saving scene camera pictures
155+
</inout_port>
156+
<inout_port
157+
name="wrist_camera_folder"
158+
default="/home/studio-user/user_ws/src/hangar_sim/wrist_camera_images"
159+
type="std::string"
160+
>
161+
Absolute path to a directory for saving wrist camera pictures
162+
</inout_port>
163+
<inout_port name="box_count" default="{box_count}" type="int">
164+
Number of boxes
165+
</inout_port>
152166
</SubTree>
153167
</TreeNodesModel>
154168
</root>

‎src/hangar_sim/objectives/createposevectorfromdirectory.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@
3333
<TreeNodesModel>
3434
<SubTree ID="CreatePoseVectorFromDirectory">
3535
<MetadataFields>
36-
<Metadata runnable="true" />
36+
<Metadata runnable="false" />
3737
<Metadata subcategory="Application - Advanced Examples" />
3838
</MetadataFields>
3939
<inout_port

‎src/hangar_sim/objectives/order_poses.xml‎

Lines changed: 18 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -6,12 +6,15 @@
66
_favorite="false"
77
>
88
<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">
9+
<Control
10+
ID="Sequence"
11+
name="Calculate the 4 corners of the region of interest"
12+
>
13+
<Action ID="CreateFloat64Message" data="{x_extent}" float64="{x_msg}" />
14+
<Action ID="UnpackFloat64Message" data="{x_extent}" float64="{x_msg}" />
15+
<Action ID="CreateFloat64Message" data="{y_extent}" float64="{y_msg}" />
16+
<Action ID="UnpackFloat64Message" data="{y_extent}" float64="{y_msg}" />
17+
<Action ID="Script" code="poses_ordered:=poses_in" />
1518
<Action
1619
ID="UnpackPoseStampedMessage"
1720
header="{header}"
@@ -35,6 +38,11 @@
3538
<Action ID="Script" code="right:=x+(x_extent/2)" />
3639
<Action ID="Script" code="top:=y+(y_extent/2)" />
3740
<Action ID="Script" code="bottom:=y-(y_extent/2)" />
41+
</Control>
42+
<Control
43+
ID="Sequence"
44+
name="Create a StampedPose for each of the region of interest's corners"
45+
>
3846
<Control ID="Sequence">
3947
<Action
4048
ID="CreatePointMessage"
@@ -120,7 +128,10 @@
120128
/>
121129
</Control>
122130
</Control>
123-
<Control ID="Sequence">
131+
<Control
132+
ID="Sequence"
133+
name="From our given set of poses, find one that is closes to each of our region if interest's conrners"
134+
>
124135
<Decorator
125136
ID="ForEach"
126137
index="{index}"

0 commit comments

Comments
 (0)