Skip to content

Commit 1af2800

Browse files
author
Bilal Gill
committed
Finalize Objectives
1 parent 5579a14 commit 1af2800

File tree

4 files changed

+46
-145
lines changed

4 files changed

+46
-145
lines changed

‎src/hangar_sim/objectives/count_boxes.xml‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,15 @@
88
<Action ID="CropPosesInBox" crop_box_centroid_pose="{crop_pose_centroid}" poses="{pose_vector}" poses_cropped="{path_cropped}" crop_box_size="12;12;1"/>
99
<Action ID="VisualizePath" marker_lifetime="0.000000" marker_name="path_cropped" path="{path_cropped}" pose_marker_size="1" show_poses="true"/>
1010
<Action ID="SwitchController" activate_asap="true" automatic_deactivation="true" controller_list_action_name="/controller_manager/list_controllers" controller_switch_action_name="/controller_manager/switch_controller" strictness="1" timeout="-1.000000" activate_controllers="platform_velocity_controller_nav2" deactivate_controllers="platform_velocity_controller"/>
11+
<Action ID="Script" code="total_box_count:=0"/>
1112
<Decorator ID="ForEach" index="{index}" out="{out}" vector_in="{path_cropped}">
12-
<Action ID="NavigateToPoseAction" action_name="/navigate_to_pose" current_pose="{current_pose}" distance_remaining="{distance_remaining}" estimated_time_remaining="{estimated_time_remaining}" ignore_stamp_time="true" navigation_time="{navigation_time}" number_of_recoveries="{number_of_recoveries}" pose_stamped="{out}" timeout_sec="-1.000000" behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"/>
13+
<Control ID="Sequence">
14+
<Action ID="NavigateToPoseAction" action_name="/navigate_to_pose" current_pose="{current_pose}" distance_remaining="{distance_remaining}" estimated_time_remaining="{estimated_time_remaining}" ignore_stamp_time="true" navigation_time="{navigation_time}" number_of_recoveries="{number_of_recoveries}" pose_stamped="{out}" timeout_sec="-1.000000" behavior_tree_path="/opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"/>
15+
<SubTree ID="Count boxes subtree" _collapsed="false" box_count="{box_count}"/>
16+
<Action ID="Script" code="total_box_count+=box_count"/>
17+
</Control>
1318
</Decorator>
19+
<Action ID="LogMessage" log_level="info" message="{total_box_count}"/>
1420
</Control>
1521
</BehaviorTree>
1622
<TreeNodesModel>

‎src/hangar_sim/objectives/count_boxes_subtree.xml‎

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

‎src/hangar_sim/objectives/test_count_boxes_subtree.xml‎

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,13 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
1+
<?xml version='1.0' encoding='UTF-8'?>
22
<root BTCPP_format="4" main_tree_to_execute="Test count boxes subtree">
3-
<BehaviorTree
4-
ID="Test count boxes subtree"
5-
_description="Objective to test the count boxes subtree"
6-
_favorite="false"
7-
>
3+
<BehaviorTree ID="Test count boxes subtree" _description="Objective to test the count boxes subtree" _favorite="false">
84
<Control ID="Sequence" name="TopLevelSequence">
9-
<SubTree ID="Navigate to Clicked Point" _collapsed="true" />
10-
<SubTree ID="Count boxes subtree" _collapsed="true" />
11-
<Action ID="LogMessage" log_level="error" message="{box_count}" />
5+
<SubTree ID="Navigate to Clicked Point" _collapsed="true"/>
6+
<SubTree ID="Count boxes subtree" _collapsed="true"/>
7+
<Action ID="LogMessage" log_level="error" message="{box_count}"/>
128
</Control>
139
</BehaviorTree>
1410
<TreeNodesModel>
15-
<SubTree ID="Test count boxes subtree" />
11+
<SubTree ID="Test count boxes subtree"/>
1612
</TreeNodesModel>
1713
</root>

‎src/hangar_sim/objectives/verify_box_count.xml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<!--//////////-->
44
<BehaviorTree ID="Verify Box Count" _description="User verify the number of boxes" _favorite="false">
55
<Control ID="Sequence" name="TopLevelSequence">
6-
<Action ID="GetFilenamesFromDirectory" file_type=".png" result="{image_filename_vector}" directory_path="/home/henry/user_ws/src/wrist_camera_images"/>
6+
<Action ID="GetFilenamesFromDirectory" file_type=".png" result="{image_filename_vector}" directory_path="/home/bilal/user_ws/src/wrist_camera_images"/>
77
<Action ID="Script" code="inc:=0"/>
88
<Decorator ID="ForEach" index="{index}" out="{out}" vector_in="{image_filename_vector}">
99
<Control ID="Sequence">
@@ -25,7 +25,7 @@
2525
</Decorator>
2626
</Control>
2727
</Decorator>
28-
<Action ID="LogMessage" log_level="error" message="{inc}"/>
28+
<Action ID="LogMessage" log_level="info" message="{inc}"/>
2929
</Control>
3030
</BehaviorTree>
3131
<TreeNodesModel>

0 commit comments

Comments
 (0)