Skip to content

Commit 41dd3ea

Browse files
MikeWrockMikeWrock
authored
Merge pull request #11 from PickNikRobotics/fix-example-behaviors-package-depend
Fix depend of moveit_studio_behavior
2 parents 350c75b + d43221b commit 41dd3ea

File tree

6 files changed

+123
-12
lines changed

6 files changed

+123
-12
lines changed

‎src/example_behaviors/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<depend>moveit_ros_planning_interface</depend>
2020
<depend>moveit_studio_vision_msgs</depend>
2121
<depend>moveit_studio_vision</depend>
22+
<depend>moveit_studio_behavior</depend>
2223

2324
<test_depend>ament_lint_auto</test_depend>
2425
<test_depend>ament_cmake_gtest</test_depend>

‎src/lab_sim/objectives/apriltag_detection_config.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ DetectAprilTags:
33
apriltag_size: 0.04
44
max_hamming: 0
55
n_threads: 1
6-
quad_decimate: 1
7-
quad_sigma: 0.1
8-
refine_edges: false
6+
quad_decimate: 2
7+
quad_sigma: 0.0
8+
refine_edges: true
99
z_up: false

‎src/lab_sim/objectives/apriltag_pick_object.xml‎

Lines changed: 30 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
23
<!--//////////-->
34
<BehaviorTree
@@ -8,7 +9,7 @@
89
<Control ID="Sequence" name="TopLevelSequence">
910
<SubTree ID="Look at table" _collapsed="true" />
1011
<Action ID="ClearSnapshot" />
11-
<SubTree ID="Wrist camera snapshot" _collapsed="true" />
12+
<SubTree ID="Take wrist camera snapshot" _collapsed="true" />
1213
<Control ID="Sequence" name="Setup">
1314
<SubTree ID="Open Gripper" />
1415
</Control>
@@ -27,19 +28,39 @@
2728
topic_name="/wrist_camera/color"
2829
message_out="{image}"
2930
/>
31+
<SubTree
32+
ID="Get AprilTag Pose from Image"
33+
image="{image}"
34+
camera_info="{camera_info}"
35+
parameters="{parameters}"
36+
_collapsed="true"
37+
detection_pose="{detection_pose}"
38+
/>
3039
<Action
31-
ID="DetectAprilTags"
40+
ID="TransformPose"
41+
output_pose="{view_pose}"
42+
input_pose="{detection_pose}"
43+
quaternion_xyzw="0;0;0;1"
44+
translation_xyz="0;-0.1;-0.15"
45+
/>
46+
<SubTree
47+
ID="Plan Move To Pose"
48+
target_pose="{view_pose}"
49+
move_to_pose_solution="{move_to_pose_solution}"
50+
/>
51+
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
52+
<SubTree
53+
ID="Get AprilTag Pose from Image"
3254
image="{image}"
3355
camera_info="{camera_info}"
3456
parameters="{parameters}"
35-
detections="{detections}"
57+
_collapsed="true"
58+
detection_pose="{detection_pose}"
3659
/>
37-
<Action ID="GetDetectionPose" />
38-
<Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
3960
<Action
4061
ID="TransformPose"
4162
output_pose="{offset_pose}"
42-
input_pose="{output_pose}"
63+
input_pose="{detection_pose}"
4364
quaternion_xyzw="0;0;0;1"
4465
translation_xyz="0;0;-0.01"
4566
/>
@@ -68,4 +89,7 @@
6889
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
6990
</Control>
7091
</BehaviorTree>
92+
<TreeNodesModel>
93+
<SubTree ID="Pick April Tag Labeled Object" />
94+
</TreeNodesModel>
7195
</root>

‎src/lab_sim/objectives/get_april_tag_pose_from_image.xml‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
<root BTCPP_format="4" main_tree_to_execute="Get AprilTag Pose from Image">
2+
<!-- ////////// -->
3+
<BehaviorTree ID="Get AprilTag Pose from Image">
4+
<Control ID="Sequence">
5+
<!--Get tag pose estimate in world frame-->
6+
<Action
7+
ID="DetectAprilTags"
8+
image="{image}"
9+
camera_info="{camera_info}"
10+
parameters="{parameters}"
11+
detections="{detections}"
12+
/>
13+
<Action
14+
ID="GetImage"
15+
topic_name="/wrist_camera/color"
16+
message_out="{image}"
17+
/>
18+
<Action
19+
ID="DetectAprilTags"
20+
image="{image}"
21+
camera_info="{camera_info}"
22+
parameters="{parameters}"
23+
detections="{detections}"
24+
/>
25+
<Action ID="GetDetectionPose" />
26+
<Action
27+
ID="TransformPoseFrame"
28+
input_pose="{detection_pose}"
29+
output_pose="{detection_pose}"
30+
/>
31+
</Control>
32+
</BehaviorTree>
33+
<TreeNodesModel>
34+
<SubTree ID="Get AprilTag Pose from Image">
35+
<input_port name="image" default="{image}" />
36+
<input_port name="camera_info" default="{camera_info}" />
37+
<input_port name="parameters" default="{parameters}" />
38+
<output_port name="detection_pose" default="{detection_pose}" />
39+
</SubTree>
40+
</TreeNodesModel>
41+
</root>

‎src/lab_sim/objectives/plan_move_to_pose.xml‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
<root BTCPP_format="4" main_tree_to_execute="Plan Move To Pose">
2+
<!-- ////////// -->
3+
<BehaviorTree ID="Plan Move To Pose">
4+
<Control ID="Sequence">
5+
<Action
6+
ID="InitializeMTCTask"
7+
task_id="move_to_pose"
8+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
9+
task="{move_to_pose_task}"
10+
/>
11+
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}" />
12+
<Action
13+
ID="SetupMTCMoveToPose"
14+
ik_frame="grasp_link"
15+
planning_group_name="manipulator"
16+
target_pose="{target_pose}"
17+
task="{move_to_pose_task}"
18+
planner_interface="moveit_default"
19+
/>
20+
<Action
21+
ID="PlanMTCTask"
22+
solution="{move_to_pose_solution}"
23+
task="{move_to_pose_task}"
24+
/>
25+
</Control>
26+
</BehaviorTree>
27+
<TreeNodesModel>
28+
<SubTree ID="Plan Move To Pose">
29+
<input_port name="target_pose" default="{target_pose}" />
30+
<output_port
31+
name="move_to_pose_solution"
32+
default="{move_to_pose_solution}"
33+
/>
34+
</SubTree>
35+
</TreeNodesModel>
36+
</root>

‎src/lab_sim/objectives/wrist_snap.xml‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,22 @@
1-
<root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute=" Take wrist camera snapshot">
23
<!--//////////-->
3-
<BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
4+
<BehaviorTree
5+
ID="Take wrist camera snapshot"
6+
_description=""
7+
_favorite="true"
8+
>
49
<Control ID="Sequence" name="TopLevelSequence">
510
<!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
611
<Action ID="GetPointCloud" topic_name="/wrist_camera/points" />
712
<Action ID="SendPointCloudToUI" />
813
</Control>
914
</BehaviorTree>
1015
<TreeNodesModel>
11-
<SubTree ID="Wrist camera snapshot" />
16+
<SubTree ID="Take wrist camera snapshot">
17+
<MetadataFields>
18+
<Metadata subcategory="Application - Basic Examples" />
19+
</MetadataFields>
20+
</SubTree>
1221
</TreeNodesModel>
1322
</root>

0 commit comments

Comments
 (0)