Skip to content

Commit 418c953

Browse files
bgill92bgill92
authored
Merge pull request #426 from PickNikRobotics/bilal_henry_hackathon_2025
Hackathon count boxes objective
2 parents d001ee9 + d011fc9 commit 418c953

21 files changed

+1000
-42
lines changed

‎src/hangar_sim/images/box.png‎

28.3 KB
Loading

‎src/hangar_sim/images/box_2.png‎

1.56 MB
Loading

‎src/hangar_sim/objectives/count_boxes.xml‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Count Boxes">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Count Boxes"
6+
_description="Count the number of boxes in a given area."
7+
_favorite="false"
8+
name="Show images taken by the robot to a human for box count verification"
9+
>
10+
<Control ID="Sequence" name="TopLevelSequence">
11+
<Action
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"
15+
/>
16+
<SubTree
17+
ID="Count Boxes Objective"
18+
_collapsed="true"
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"
22+
/>
23+
</Control>
24+
</BehaviorTree>
25+
<TreeNodesModel>
26+
<SubTree ID="Count Boxes">
27+
<MetadataFields>
28+
<Metadata runnable="true" />
29+
<Metadata subcategory="Application - Advanced Examples" />
30+
</MetadataFields>
31+
</SubTree>
32+
</TreeNodesModel>
33+
</root>

‎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: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Count boxes subtree">
3+
<BehaviorTree
4+
ID="Count boxes subtree"
5+
_description="Counts the boxes visible in a scene"
6+
_favorite="false"
7+
>
8+
<Control ID="Fallback">
9+
<Control ID="Sequence" name="TopLevelSequence">
10+
<Action
11+
ID="GetSyncedImages"
12+
camera_info_1="{camera_info_1}"
13+
camera_info_1_topic_name="/wrist_camera/camera_info"
14+
camera_info_2="{camera_info_2}"
15+
camera_info_2_topic_name="/scene_camera/camera_info"
16+
image_1="{wrist_camera_image}"
17+
image_1_topic_name="/wrist_camera/color"
18+
image_2="{scene_camera_image}"
19+
image_2_topic_name="/scene_camera/color"
20+
/>
21+
<Action
22+
ID="SaveImageToFile"
23+
file_path="{wrist_camera_folder}"
24+
file_prefix="box"
25+
image="{wrist_camera_image}"
26+
/>
27+
<Action
28+
ID="SaveImageToFile"
29+
file_path="{scene_camera_folder}"
30+
file_prefix="box"
31+
image="{scene_camera_image}"
32+
/>
33+
<SubTree
34+
ID="Segment Image from Text Prompt Subtree"
35+
_collapsed="true"
36+
clip_model_path="models/clip.onnx"
37+
clipseg_model_path="models/clipseg.onnx"
38+
erosion_size="0"
39+
image_topic_name="/wrist_camera/color"
40+
masks_visualization_topic="/masks_visualization"
41+
masks2d="{masks2d}"
42+
model_package="moveit_pro_clipseg"
43+
prompts="small boxes"
44+
negative_prompts="grey"
45+
threshold="0.4"
46+
/>
47+
<Action
48+
ID="PublishMask2D"
49+
image="{wrist_camera_image}"
50+
masks="{masks2d}"
51+
masks_visualization_topic="/masks_visualization"
52+
opacity="0.500000"
53+
bounding_box_detection_class="box"
54+
_skipIf="true"
55+
/>
56+
<Action
57+
ID="SwitchUIPrimaryView"
58+
primary_view_name="/masks_visualization"
59+
_skipIf="true"
60+
/>
61+
<Action
62+
ID="GetPointCloud"
63+
message_out="{point_cloud}"
64+
publisher_timeout_sec="5.000000"
65+
timeout_sec="5.000000"
66+
topic_name="/wrist_camera/points"
67+
/>
68+
<Action
69+
ID="CreateStampedPose"
70+
reference_frame="ridgeback_base_link"
71+
stamped_pose="{robot_pose}"
72+
orientation_xyzw="0;0;0;1"
73+
position_xyz="0;0;0"
74+
/>
75+
<Action
76+
ID="FindMaskedObjects"
77+
base_frame="world"
78+
camera_info="{camera_info_1}"
79+
detected_shapes="{detected_shapes}"
80+
face_separation_threshold="0.010000"
81+
masks2d="{masks2d}"
82+
minimum_face_area="0.000625"
83+
plane_inlier_threshold="0.010000"
84+
point_cloud="{point_cloud}"
85+
/>
86+
<Action ID="Script" code="box_count:=0" />
87+
<Decorator
88+
ID="ForEach"
89+
index="{index}"
90+
out="{out}"
91+
vector_in="{detected_shapes}"
92+
>
93+
<Control ID="Sequence">
94+
<Control ID="IfThenElse">
95+
<Control ID="Sequence">
96+
<Action
97+
ID="ExtractGraspableObjectPose"
98+
graspable_object="{out}"
99+
pose="{pose_stamped}"
100+
/>
101+
<Action
102+
ID="CreateGraspableObject"
103+
cuboid_object="{reference_cuboid}"
104+
dx="0.25"
105+
dy="0.25"
106+
dz="0.25"
107+
generate_front_face="true"
108+
generate_side_faces="true"
109+
generate_top_face="true"
110+
object_id="reference_cuboid"
111+
pose="{pose_stamped}"
112+
/>
113+
<Action
114+
ID="CheckCuboidSimilarity"
115+
base_frame="world"
116+
distance_threshold="0.020000"
117+
input_cuboid="{out}"
118+
orientation_threshold="3.142"
119+
reference_cuboid="{reference_cuboid}"
120+
/>
121+
</Control>
122+
<Action ID="Script" code="box_count+=1" />
123+
<Action
124+
ID="AlwaysSuccess"
125+
name="If the cuboid isn't similar, just don't count it"
126+
/>
127+
</Control>
128+
</Control>
129+
</Decorator>
130+
<Action
131+
ID="ResetPlanningSceneObjects"
132+
apply_planning_scene_service="/apply_planning_scene"
133+
name="CreateGraspableObject adds to the planning scene but we don't want that"
134+
/>
135+
</Control>
136+
<Action
137+
ID="Script"
138+
code="box_count:=0"
139+
name="This can happen if CLIPSeg doesn't find any boxes"
140+
/>
141+
</Control>
142+
</BehaviorTree>
143+
<TreeNodesModel>
144+
<SubTree ID="Count boxes subtree">
145+
<MetadataFields>
146+
<Metadata runnable="False" />
147+
<Metadata subcategory="Perception - ML" />
148+
</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>
166+
</SubTree>
167+
</TreeNodesModel>
168+
</root>

‎src/hangar_sim/objectives/createposevectorfromdirectory.xml‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="CreatePoseVectorFromDirectory">
3+
<BehaviorTree
4+
ID="CreatePoseVectorFromDirectory"
5+
_description=""
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<Action
10+
ID="GetFilenamesFromDirectory"
11+
result="{result}"
12+
directory_path="{directory_path}"
13+
file_type=".yaml"
14+
/>
15+
<Action ID="ResetVector" vector="{pose_vector}" />
16+
<Decorator ID="ForEach" index="{index}" out="{out}" vector_in="{result}">
17+
<Control ID="Sequence">
18+
<Action
19+
ID="LoadPoseStampedFromYaml"
20+
output="{pose_msg}"
21+
file_path="{out}"
22+
/>
23+
<Action
24+
ID="PushBackVector"
25+
input_vector="{pose_vector}"
26+
output_vector="{pose_vector}"
27+
element="{pose_msg}"
28+
/>
29+
</Control>
30+
</Decorator>
31+
</Control>
32+
</BehaviorTree>
33+
<TreeNodesModel>
34+
<SubTree ID="CreatePoseVectorFromDirectory">
35+
<MetadataFields>
36+
<Metadata runnable="false" />
37+
<Metadata subcategory="Application - Advanced Examples" />
38+
</MetadataFields>
39+
<inout_port
40+
name="directory_path"
41+
default="{directory_path}"
42+
type="std::string"
43+
/>
44+
<inout_port name="pose_vector" default="{pose_vector}" />
45+
</SubTree>
46+
</TreeNodesModel>
47+
</root>

0 commit comments

Comments
 (0)