Skip to content

Commit 0a58051

Browse files
noah-wardlowshaur-k
noah-wardlow
authored and
shaur-k
committed
lab_sim: add Inspector multi-view inspection objective (images + merged point clouds)
1 parent 5b7fe6f commit 0a58051

File tree

1 file changed

+85
-0
lines changed

1 file changed

+85
-0
lines changed

‎src/lab_sim/objectives/inspector.xml‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<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">
4+
<Control ID="Sequence">
5+
<!--Initialize visualization and point cloud vector-->
6+
<SubTree ID="Clear Snapshot"/>
7+
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/>
8+
<!--Initialize empty point cloud vector-->
9+
<Action ID="Script" code="cloud_count := 0"/>
10+
<!--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}"/>
17+
<!--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}">
19+
<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}"/>
24+
</Control>
25+
</Decorator>
26+
<!--Convert solution for trajectory analysis/logging-->
27+
<Action ID="ConvertMtcSolutionToJointTrajectory" solution="{first_solution}" joint_trajectory="{joint_traj}"/>
28+
<!--Use ForEach to visit all inspection points and gather data-->
29+
<Decorator ID="ForEach" vector_in="{inspection_poses}" out="{current_pose}" index="{capture_index}">
30+
<Control ID="Sequence">
31+
<!--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}"/>
37+
<!--Visualize current inspection point-->
38+
<Action ID="VisualizePose" marker_name="inspection_point" pose="{current_pose}" marker_lifetime="10.0" marker_size="0.05"/>
39+
<!--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"/>
42+
<!--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}"/>
47+
<!--Debug: publish individual cloud and current vector size-->
48+
<Action ID="SendPointCloudToUI" point_cloud="{transformed_cloud}" pcd_topic="/pcd_individual_capture"/>
49+
</Control>
50+
</Decorator>
51+
<!--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"/>
53+
<!--Publish merged point cloud to UI for visualization-->
54+
<Action ID="SendPointCloudToUI" point_cloud="{merged_cloud}" pcd_topic="/pcd_pointcloud_captures"/>
55+
<!--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}"/>
60+
<!--Return to home position-->
61+
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="joint_trajectory_controller"/>
62+
</Control>
63+
</BehaviorTree>
64+
<TreeNodesModel>
65+
<SubTree ID="Inspector">
66+
<MetadataFields>
67+
<Metadata subcategory="Application - Basic Examples"/>
68+
<Metadata runnable="true"/>
69+
</MetadataFields>
70+
<!--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}"/>
83+
</SubTree>
84+
</TreeNodesModel>
85+
</root>

0 commit comments

Comments
 (0)