Skip to content

Commit 661c3ad

Browse files
pac48pac48
committed
Add Register Machined Part Annotated objective
Signed-off-by: Paul Gesel <[email protected]>
1 parent a4e7026 commit 661c3ad

File tree

1 file changed

+155
-0
lines changed

1 file changed

+155
-0
lines changed

‎src/grinding_sim/objectives/register_machined_part_annotated.xml

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Register Machined Part Annotated">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Register Machined Part Annotated"
6+
_description=""
7+
_favorite="true"
8+
registered_pose="{registered_pose}"
9+
>
10+
<Control ID="Sequence" name="TopLevelSequence">
11+
<Action ID="ClearSnapshot" />
12+
<SubTree ID="Take Snapshot" _collapsed="true" />
13+
<Action
14+
ID="CreateStampedPose"
15+
reference_frame="world"
16+
stamped_pose="{world_pose}"
17+
/>
18+
<Action
19+
ID="CreateStampedPose"
20+
position_xyz="0.2;-1.5;1.0"
21+
orientation_xyzw="0.000;0.000;0.3827;0.9239"
22+
name="Initial Guess"
23+
stamped_pose="{guess_pose}"
24+
reference_frame="world"
25+
/>
26+
<Action
27+
ID="ExamplePublishArrowMarker"
28+
start_pose="{world_pose}"
29+
end_pose="{guess_pose}"
30+
/>
31+
<Action
32+
ID="ExamplePublishTextMarker"
33+
scale="2"
34+
pose="{world_pose}"
35+
message="world to initial guess"
36+
position_xyz="0.2;-0.6;1.0"
37+
/>
38+
<SubTree
39+
ID="Load Engine Block Pointcloud"
40+
_collapsed="true"
41+
color="0;0;255"
42+
initial_pose="{guess_pose}"
43+
engine_block_cloud="{blue_engine_block_cloud}"
44+
/>
45+
<Action
46+
ID="SendPointCloudToUI"
47+
point_cloud="{blue_engine_block_cloud}"
48+
pcd_topic="/pcd_pointcloud_captures"
49+
/>
50+
<Action ID="BreakpointSubscriber" breakpoint_topic="/studio_breakpoint" />
51+
<Action
52+
ID="GetPointCloud"
53+
topic_name="/scene_camera/points"
54+
message_out="{scene_point_cloud}"
55+
timeout_sec="5.000000"
56+
/>
57+
<Action
58+
ID="CreateStampedPose"
59+
reference_frame="world"
60+
stamped_pose="{centroid_pose}"
61+
position_xyz="0;-1.69;1.0"
62+
/>
63+
<Action
64+
ID="CropPointsInBox"
65+
crop_box_centroid_pose="{centroid_pose}"
66+
point_cloud="{scene_point_cloud}"
67+
point_cloud_cropped="{cropped_scene_point_cloud}"
68+
crop_box_size="1.0;0.5;0.55"
69+
/>
70+
<Action
71+
ID="TransformPointCloudFrame"
72+
input_cloud="{cropped_scene_point_cloud}"
73+
output_cloud="{cropped_scene_point_cloud}"
74+
target_frame="world"
75+
/>
76+
<Action
77+
ID="SendPointCloudToUI"
78+
point_cloud="{cropped_scene_point_cloud}"
79+
pcd_topic="/pcd_pointcloud_captures"
80+
/>
81+
<Action
82+
ID="RegisterPointClouds"
83+
target_point_cloud="{cropped_scene_point_cloud}"
84+
base_point_cloud="{blue_engine_block_cloud}"
85+
max_iterations="100"
86+
target_pose_in_base_frame="{registered_to_guess_pose}"
87+
max_correspondence_distance="0.5"
88+
/>
89+
<Action
90+
ID="TransformPoseWithPose"
91+
input_pose="{guess_pose}"
92+
transform_pose="{registered_to_guess_pose}"
93+
output_pose="{registered_pose}"
94+
/>
95+
<Action
96+
ID="ExamplePublishArrowMarker"
97+
start_pose="{guess_pose}"
98+
end_pose="{registered_pose}"
99+
/>
100+
<Action
101+
ID="ExamplePublishTextMarker"
102+
scale="2"
103+
pose="{guess_pose}"
104+
message="registration correction"
105+
position_xyz="0;0;-0.2"
106+
/>
107+
<Action ID="BreakpointSubscriber" breakpoint_topic="/studio_breakpoint" />
108+
<SubTree
109+
ID="Load Engine Block Pointcloud"
110+
_collapsed="true"
111+
color="0;255;0"
112+
initial_pose="{registered_pose}"
113+
engine_block_cloud="{green_engine_block_cloud}"
114+
/>
115+
<Action ID="ClearSnapshot" />
116+
<Action
117+
ID="VisualizePose"
118+
marker_lifetime="0.000000"
119+
marker_name="registered_pose"
120+
marker_size="0.100000"
121+
pose="{registered_pose}"
122+
/>
123+
<Action
124+
ID="ExamplePublishArrowMarker"
125+
start_pose="{world_pose}"
126+
end_pose="{registered_pose}"
127+
/>
128+
<Action
129+
ID="ExamplePublishTextMarker"
130+
scale="2"
131+
pose="{world_pose}"
132+
message="model to world"
133+
position_xyz="0.2;-0.6;0.5"
134+
/>
135+
<Action
136+
ID="SendPointCloudToUI"
137+
point_cloud="{green_engine_block_cloud}"
138+
pcd_topic="/pcd_pointcloud_captures"
139+
/>
140+
</Control>
141+
</BehaviorTree>
142+
<TreeNodesModel>
143+
<SubTree ID="Register Machined Part Annotated">
144+
<output_port name="registered_pose" default="{registered_pose}" />
145+
<MetadataFields>
146+
<Metadata subcategory="Perception - 3D Point Cloud" />
147+
<Metadata runnable="true" />
148+
</MetadataFields>
149+
<MetadataFields>
150+
<Metadata subcategory="Perception - 3D Point Cloud" />
151+
<Metadata runnable="true" />
152+
</MetadataFields>
153+
</SubTree>
154+
</TreeNodesModel>
155+
</root>

0 commit comments

Comments
 (0)