Skip to content

Commit 442391f

Browse files
noah-wardlownoah-wardlow
authored
Merge pull request #420 from PickNikRobotics/feature/lab-sim-inspector-objective
Feature/lab sim inspector objective
2 parents 1613ef2 + 3803351 commit 442391f

File tree

2 files changed

+225
-0
lines changed

2 files changed

+225
-0
lines changed

‎src/lab_sim/objectives/inspector.xml‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,224 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Inspector">
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+
>
8+
<Control ID="Sequence">
9+
<!--Initialize visualization and point cloud vector-->
10+
<SubTree ID="Clear Snapshot" />
11+
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
12+
<!--Initialize empty point cloud vector-->
13+
<Action ID="Script" code="cloud_count := 0" />
14+
<!--Create inspection viewpoints around target area-->
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+
/>
51+
<!--Use ForEachUntilSuccess to find first reachable inspection point (but don't execute yet)-->
52+
<Decorator
53+
ID="ForEachUntilSuccess"
54+
vector_in="{inspection_poses}"
55+
out="{reachable_pose}"
56+
index="{pose_index}"
57+
>
58+
<Control ID="Sequence">
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+
/>
78+
</Control>
79+
</Decorator>
80+
<!--Convert solution for trajectory analysis/logging-->
81+
<Action
82+
ID="ConvertMtcSolutionToJointTrajectory"
83+
solution="{first_solution}"
84+
joint_trajectory="{joint_traj}"
85+
/>
86+
<!--Use ForEach to visit all inspection points and gather data-->
87+
<Decorator
88+
ID="ForEach"
89+
vector_in="{inspection_poses}"
90+
out="{current_pose}"
91+
index="{capture_index}"
92+
>
93+
<Control ID="Sequence">
94+
<!--Move to inspection point-->
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}" />
115+
<!--Visualize current inspection point-->
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+
/>
123+
<!--Capture and save inspection image-->
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+
/>
135+
<!--Capture point cloud for 3D reconstruction-->
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+
/>
154+
<!--Debug: publish individual cloud and current vector size-->
155+
<Action
156+
ID="SendPointCloudToUI"
157+
point_cloud="{transformed_cloud}"
158+
pcd_topic="/pcd_individual_capture"
159+
/>
160+
</Control>
161+
</Decorator>
162+
<!--Merge all point clouds for complete 3D view-->
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+
/>
170+
<!--Publish merged point cloud to UI for visualization-->
171+
<Action
172+
ID="SendPointCloudToUI"
173+
point_cloud="{merged_cloud}"
174+
pcd_topic="/pcd_pointcloud_captures"
175+
/>
176+
<!--Demonstrate SetupMTCFromSolution for repeatable inspection routine-->
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}" />
194+
<!--Return to home position-->
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+
/>
201+
</Control>
202+
</BehaviorTree>
203+
<TreeNodesModel>
204+
<SubTree ID="Inspector">
205+
<MetadataFields>
206+
<Metadata subcategory="Application - Basic Examples" />
207+
<Metadata runnable="true" />
208+
</MetadataFields>
209+
<!--Output ports-->
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}" />
222+
</SubTree>
223+
</TreeNodesModel>
224+
</root>

‎src/lab_sim/test/objectives_integration_test.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,7 @@
7171
"Record Square Trajectory",
7272
"Stack Blocks with ICP", # Skipped because there is no primary ui to switch to in ci
7373
"Teleoperate",
74+
"Inspector",
7475
}
7576

7677
# This is a workaround to avoid the test running before ros control is ready

0 commit comments

Comments
 (0)