Skip to content

Commit 0993699

Browse files
noah-wardlowshaur-k
noah-wardlow
authored and
shaur-k
committed
Update inspector.xml
1 parent 0a58051 commit 0993699

File tree

1 file changed

+191
-52
lines changed

1 file changed

+191
-52
lines changed

‎src/lab_sim/objectives/inspector.xml‎

Lines changed: 191 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -1,85 +1,224 @@
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="Inspector">
3-
<BehaviorTree ID="Inspector" _description="Multi-view inspection routine: Captures images and point clouds from multiple viewpoints for 3D reconstruction or quality inspection" _favorite="true">
3+
<BehaviorTree
4+
ID="Inspector"
5+
_description="Multi-view inspection routine: Captures images and point clouds from multiple viewpoints for 3D reconstruction or quality inspection"
6+
_favorite="true"
7+
>
48
<Control ID="Sequence">
59
<!--Initialize visualization and point cloud vector-->
6-
<SubTree ID="Clear Snapshot"/>
7-
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
10+
<SubTree ID="Clear Snapshot" />
11+
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
812
<!--Initialize empty point cloud vector-->
9-
<Action ID="Script" code="cloud_count := 0"/>
13+
<Action ID="Script" code="cloud_count := 0" />
1014
<!--Create inspection viewpoints around target area-->
11-
<Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0.5;0.3;0.5" stamped_pose="{pose1}" reference_frame="world"/>
12-
<Action ID="AddPoseStampedToVector" vector="{inspection_poses}" input="{pose1}"/>
13-
<Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0.6;0.4;0.5" stamped_pose="{pose2}" reference_frame="world"/>
14-
<Action ID="AddPoseStampedToVector" vector="{inspection_poses}" input="{pose2}"/>
15-
<Action ID="CreateStampedPose" orientation_xyzw="0;-1;0;0" position_xyz="0.4;0.5;0.5" stamped_pose="{pose3}" reference_frame="world"/>
16-
<Action ID="AddPoseStampedToVector" vector="{inspection_poses}" input="{pose3}"/>
15+
<Action
16+
ID="CreateStampedPose"
17+
orientation_xyzw="0;-1;0;0"
18+
position_xyz="0.5;0.3;0.5"
19+
stamped_pose="{pose1}"
20+
reference_frame="world"
21+
/>
22+
<Action
23+
ID="AddPoseStampedToVector"
24+
vector="{inspection_poses}"
25+
input="{pose1}"
26+
/>
27+
<Action
28+
ID="CreateStampedPose"
29+
orientation_xyzw="0;-1;0;0"
30+
position_xyz="0.6;0.4;0.5"
31+
stamped_pose="{pose2}"
32+
reference_frame="world"
33+
/>
34+
<Action
35+
ID="AddPoseStampedToVector"
36+
vector="{inspection_poses}"
37+
input="{pose2}"
38+
/>
39+
<Action
40+
ID="CreateStampedPose"
41+
orientation_xyzw="0;-1;0;0"
42+
position_xyz="0.4;0.5;0.5"
43+
stamped_pose="{pose3}"
44+
reference_frame="world"
45+
/>
46+
<Action
47+
ID="AddPoseStampedToVector"
48+
vector="{inspection_poses}"
49+
input="{pose3}"
50+
/>
1751
<!--Use ForEachUntilSuccess to find first reachable inspection point (but don't execute yet)-->
18-
<Decorator ID="ForEachUntilSuccess" vector_in="{inspection_poses}" out="{reachable_pose}" index="{pose_index}">
52+
<Decorator
53+
ID="ForEachUntilSuccess"
54+
vector_in="{inspection_poses}"
55+
out="{reachable_pose}"
56+
index="{pose_index}"
57+
>
1958
<Control ID="Sequence">
20-
<Action ID="InitializeMTCTask" controller_names="joint_trajectory_controller" task="{mtc_task}" task_id="reach_test"/>
21-
<Action ID="SetupMTCCurrentState" task="{mtc_task}"/>
22-
<Action ID="SetupMTCPlanToPose" planning_group_name="manipulator" ik_frame="grasp_link" target_pose="{reachable_pose}" task="{mtc_task}"/>
23-
<Action ID="PlanMTCTask" solution="{first_solution}" task="{mtc_task}"/>
59+
<Action
60+
ID="InitializeMTCTask"
61+
controller_names="joint_trajectory_controller"
62+
task="{mtc_task}"
63+
task_id="reach_test"
64+
/>
65+
<Action ID="SetupMTCCurrentState" task="{mtc_task}" />
66+
<Action
67+
ID="SetupMTCPlanToPose"
68+
planning_group_name="manipulator"
69+
ik_frame="grasp_link"
70+
target_pose="{reachable_pose}"
71+
task="{mtc_task}"
72+
/>
73+
<Action
74+
ID="PlanMTCTask"
75+
solution="{first_solution}"
76+
task="{mtc_task}"
77+
/>
2478
</Control>
2579
</Decorator>
2680
<!--Convert solution for trajectory analysis/logging-->
27-
<Action ID="ConvertMtcSolutionToJointTrajectory" solution="{first_solution}" joint_trajectory="{joint_traj}"/>
81+
<Action
82+
ID="ConvertMtcSolutionToJointTrajectory"
83+
solution="{first_solution}"
84+
joint_trajectory="{joint_traj}"
85+
/>
2886
<!--Use ForEach to visit all inspection points and gather data-->
29-
<Decorator ID="ForEach" vector_in="{inspection_poses}" out="{current_pose}" index="{capture_index}">
87+
<Decorator
88+
ID="ForEach"
89+
vector_in="{inspection_poses}"
90+
out="{current_pose}"
91+
index="{capture_index}"
92+
>
3093
<Control ID="Sequence">
3194
<!--Move to inspection point-->
32-
<Action ID="InitializeMTCTask" controller_names="joint_trajectory_controller" task="{move_task}" task_id="inspect"/>
33-
<Action ID="SetupMTCCurrentState" task="{move_task}"/>
34-
<Action ID="SetupMTCPlanToPose" planning_group_name="manipulator" ik_frame="grasp_link" target_pose="{current_pose}" task="{move_task}"/>
35-
<Action ID="PlanMTCTask" solution="{move_solution}" task="{move_task}"/>
36-
<Action ID="ExecuteMTCTask" solution="{move_solution}"/>
95+
<Action
96+
ID="InitializeMTCTask"
97+
controller_names="joint_trajectory_controller"
98+
task="{move_task}"
99+
task_id="inspect"
100+
/>
101+
<Action ID="SetupMTCCurrentState" task="{move_task}" />
102+
<Action
103+
ID="SetupMTCPlanToPose"
104+
planning_group_name="manipulator"
105+
ik_frame="grasp_link"
106+
target_pose="{current_pose}"
107+
task="{move_task}"
108+
/>
109+
<Action
110+
ID="PlanMTCTask"
111+
solution="{move_solution}"
112+
task="{move_task}"
113+
/>
114+
<Action ID="ExecuteMTCTask" solution="{move_solution}" />
37115
<!--Visualize current inspection point-->
38-
<Action ID="VisualizePose" marker_name="inspection_point" pose="{current_pose}" marker_lifetime="10.0" marker_size="0.05"/>
116+
<Action
117+
ID="VisualizePose"
118+
marker_name="inspection_point"
119+
pose="{current_pose}"
120+
marker_lifetime="10.0"
121+
marker_size="0.05"
122+
/>
39123
<!--Capture and save inspection image-->
40-
<Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}" timeout_sec="5.0"/>
41-
<Action ID="SaveImageToFile" image="{image}" file_path="/tmp/inspection_image.png"/>
124+
<Action
125+
ID="GetImage"
126+
topic_name="/wrist_camera/color"
127+
message_out="{image}"
128+
timeout_sec="5.0"
129+
/>
130+
<Action
131+
ID="SaveImageToFile"
132+
image="{image}"
133+
file_path="/tmp/inspection_image.png"
134+
/>
42135
<!--Capture point cloud for 3D reconstruction-->
43-
<Action ID="Script" code="cloud_count := cloud_count + 1"/>
44-
<Action ID="GetPointCloud" topic_name="/wrist_camera/points" message_out="{current_cloud}" timeout_sec="5.0"/>
45-
<Action ID="TransformPointCloudFrame" input_cloud="{current_cloud}" output_cloud="{transformed_cloud}" target_frame="world"/>
46-
<Action ID="AddPointCloudToVector" point_cloud="{transformed_cloud}" point_cloud_vector="{point_cloud_vector}"/>
136+
<Action ID="Script" code="cloud_count := cloud_count + 1" />
137+
<Action
138+
ID="GetPointCloud"
139+
topic_name="/wrist_camera/points"
140+
message_out="{current_cloud}"
141+
timeout_sec="5.0"
142+
/>
143+
<Action
144+
ID="TransformPointCloudFrame"
145+
input_cloud="{current_cloud}"
146+
output_cloud="{transformed_cloud}"
147+
target_frame="world"
148+
/>
149+
<Action
150+
ID="AddPointCloudToVector"
151+
point_cloud="{transformed_cloud}"
152+
point_cloud_vector="{point_cloud_vector}"
153+
/>
47154
<!--Debug: publish individual cloud and current vector size-->
48-
<Action ID="SendPointCloudToUI" point_cloud="{transformed_cloud}" pcd_topic="/pcd_individual_capture"/>
155+
<Action
156+
ID="SendPointCloudToUI"
157+
point_cloud="{transformed_cloud}"
158+
pcd_topic="/pcd_individual_capture"
159+
/>
49160
</Control>
50161
</Decorator>
51162
<!--Merge all point clouds for complete 3D view-->
52-
<Action ID="MergePointClouds" grid_resolution_meters="0.005" merged_cloud="{merged_cloud}" point_clouds="{point_cloud_vector}" align_point_clouds="false"/>
163+
<Action
164+
ID="MergePointClouds"
165+
grid_resolution_meters="0.005"
166+
merged_cloud="{merged_cloud}"
167+
point_clouds="{point_cloud_vector}"
168+
align_point_clouds="false"
169+
/>
53170
<!--Publish merged point cloud to UI for visualization-->
54-
<Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
171+
<Action
172+
ID="SendPointCloudToUI"
173+
point_cloud="{merged_cloud}"
174+
pcd_topic="/pcd_pointcloud_captures"
175+
/>
55176
<!--Demonstrate SetupMTCFromSolution for repeatable inspection routine-->
56-
<Action ID="InitializeMTCTask" controller_names="joint_trajectory_controller" task="{replay_task}" task_id="repeat_inspection"/>
57-
<Action ID="SetupMTCFromSolution" task="{replay_task}" solution="{first_solution}"/>
58-
<Action ID="PlanMTCTask" solution="{replay_solution}" task="{replay_task}"/>
59-
<Action ID="ExecuteMTCTask" solution="{replay_solution}"/>
177+
<Action
178+
ID="InitializeMTCTask"
179+
controller_names="joint_trajectory_controller"
180+
task="{replay_task}"
181+
task_id="repeat_inspection"
182+
/>
183+
<Action
184+
ID="SetupMTCFromSolution"
185+
task="{replay_task}"
186+
solution="{first_solution}"
187+
/>
188+
<Action
189+
ID="PlanMTCTask"
190+
solution="{replay_solution}"
191+
task="{replay_task}"
192+
/>
193+
<Action ID="ExecuteMTCTask" solution="{replay_solution}" />
60194
<!--Return to home position-->
61-
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="joint_trajectory_controller"/>
195+
<SubTree
196+
ID="Move to Waypoint"
197+
waypoint_name="Look at Table"
198+
joint_group_name="manipulator"
199+
controller_names="joint_trajectory_controller"
200+
/>
62201
</Control>
63202
</BehaviorTree>
64203
<TreeNodesModel>
65204
<SubTree ID="Inspector">
66205
<MetadataFields>
67-
<Metadata subcategory="Application - Basic Examples"/>
68-
<Metadata runnable="true"/>
206+
<Metadata subcategory="Application - Basic Examples" />
207+
<Metadata runnable="true" />
69208
</MetadataFields>
70209
<!--Output ports-->
71-
<output_port name="inspection_poses" default="{inspection_poses}"/>
72-
<output_port name="reachable_pose" default="{reachable_pose}"/>
73-
<output_port name="first_solution" default="{first_solution}"/>
74-
<output_port name="joint_traj" default="{joint_traj}"/>
75-
<output_port name="current_pose" default="{current_pose}"/>
76-
<output_port name="move_solution" default="{move_solution}"/>
77-
<output_port name="image" default="{image}"/>
78-
<output_port name="point_cloud_vector" default="{point_cloud_vector}"/>
79-
<output_port name="merged_cloud" default="{merged_cloud}"/>
80-
<output_port name="replay_solution" default="{replay_solution}"/>
81-
<output_port name="current_cloud" default="{current_cloud}"/>
82-
<output_port name="transformed_cloud" default="{transformed_cloud}"/>
210+
<output_port name="inspection_poses" default="{inspection_poses}" />
211+
<output_port name="reachable_pose" default="{reachable_pose}" />
212+
<output_port name="first_solution" default="{first_solution}" />
213+
<output_port name="joint_traj" default="{joint_traj}" />
214+
<output_port name="current_pose" default="{current_pose}" />
215+
<output_port name="move_solution" default="{move_solution}" />
216+
<output_port name="image" default="{image}" />
217+
<output_port name="point_cloud_vector" default="{point_cloud_vector}" />
218+
<output_port name="merged_cloud" default="{merged_cloud}" />
219+
<output_port name="replay_solution" default="{replay_solution}" />
220+
<output_port name="current_cloud" default="{current_cloud}" />
221+
<output_port name="transformed_cloud" default="{transformed_cloud}" />
83222
</SubTree>
84223
</TreeNodesModel>
85224
</root>

0 commit comments

Comments
 (0)