Skip to content

Commit 9093cfb

Browse files
MikeWrockMikeWrock
authored
Merge pull request #10 from PickNikRobotics/pr-update-to-make-ports-explicit
Update ports explicitly
2 parents c99eeb4 + 23d20c4 commit 9093cfb

14 files changed

+182
-43
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/add_waypoints_to_mtc_task.xml‎

Lines changed: 28 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -3,34 +3,39 @@
33
<!--//////////-->
44
<BehaviorTree ID="Add Waypoints to MTC Task">
55
<Control ID="Sequence">
6-
<Decorator
7-
ID="ForEachString"
8-
vector_in="{waypoint_names}"
9-
out="{waypoint_name}"
10-
>
11-
<Control ID="Sequence">
12-
<Action
13-
ID="RetrieveWaypoint"
14-
waypoint_joint_state="{target_joint_state}"
15-
waypoint_name="{waypoint_name}"
16-
joint_group_name="{joint_group_name}"
17-
/>
18-
<Action
19-
ID="SetupMTCMoveToJointState"
20-
joint_state="{target_joint_state}"
21-
name="SetupMTCMoveToJointState"
22-
planning_group_name="{joint_group_name}"
23-
planner_interface="{planner_interface}"
24-
task="{mtc_task}"
25-
/>
26-
</Control>
27-
</Decorator>
6+
<Action
7+
ID="RetrieveWaypoint"
8+
waypoint_joint_state="{target_joint_state}"
9+
waypoint_name="Pick Cube"
10+
joint_group_name="{joint_group_name}"
11+
/>
12+
<Action
13+
ID="SetupMTCMoveToJointState"
14+
joint_state="{target_joint_state}"
15+
name="SetupMTCMoveToJointState"
16+
planning_group_name="{joint_group_name}"
17+
planner_interface="{planner_interface}"
18+
task="{mtc_task}"
19+
/>
20+
<Action
21+
ID="RetrieveWaypoint"
22+
waypoint_joint_state="{target_joint_state}"
23+
waypoint_name="Place Cube"
24+
joint_group_name="{joint_group_name}"
25+
/>
26+
<Action
27+
ID="SetupMTCMoveToJointState"
28+
joint_state="{target_joint_state}"
29+
name="SetupMTCMoveToJointState"
30+
planning_group_name="{joint_group_name}"
31+
planner_interface="{planner_interface}"
32+
task="{mtc_task}"
33+
/>
2834
</Control>
2935
</BehaviorTree>
3036
<TreeNodesModel>
3137
<SubTree ID="Add Waypoints to MTC Task">
3238
<input_port name="joint_group_name" default="manipulator" />
33-
<input_port name="waypoint_names" default="Pick Cube;Place Cube" />
3439
<inout_port name="mtc_task" default="{mtc_task}" />
3540
<input_port name="planner_interface" default="moveit_default" />
3641
</SubTree>

‎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/create_pose_vector.xml‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,82 +6,100 @@
66
ID="CreateStampedPose"
77
orientation_xyzw="0;-1;0;0"
88
position_xyz="1;0.5;1"
9+
stamped_pose="{stamped_pose}"
910
/>
1011
<Action
1112
ID="AddPoseStampedToVector"
1213
vector="{target_poses}"
14+
input="{stamped_pose}"
1315
/>
1416
<Action
1517
ID="CreateStampedPose"
1618
orientation_xyzw="0;-1;0;0"
1719
position_xyz="0.75;0.5;0.9"
20+
stamped_pose="{stamped_pose}"
1821
/>
1922
<Action
2023
ID="AddPoseStampedToVector"
2124
vector="{target_poses}"
25+
input="{stamped_pose}"
2226
/>
2327
<Action
2428
ID="CreateStampedPose"
2529
orientation_xyzw="0;-1;0;0"
2630
position_xyz="0.5;0.5;0.8"
31+
stamped_pose="{stamped_pose}"
2732
/>
2833
<Action
2934
ID="AddPoseStampedToVector"
3035
vector="{target_poses}"
36+
input="{stamped_pose}"
3137
/>
3238
<Action
3339
ID="CreateStampedPose"
3440
orientation_xyzw="0;-1;0;0"
3541
position_xyz="0.25;0.5;0.7"
42+
stamped_pose="{stamped_pose}"
3643
/>
3744
<Action
3845
ID="AddPoseStampedToVector"
3946
vector="{target_poses}"
47+
input="{stamped_pose}"
4048
/>
4149
<Action
4250
ID="CreateStampedPose"
4351
orientation_xyzw="0;-1;0;0"
4452
position_xyz="0;0.5;0.6"
53+
stamped_pose="{stamped_pose}"
4554
/>
4655
<Action
4756
ID="AddPoseStampedToVector"
4857
vector="{target_poses}"
58+
input="{stamped_pose}"
4959
/>
5060
<Action
5161
ID="CreateStampedPose"
5262
orientation_xyzw="0;-1;0;0"
5363
position_xyz="-0.25;0.5;0.7"
64+
stamped_pose="{stamped_pose}"
5465
/>
5566
<Action
5667
ID="AddPoseStampedToVector"
5768
vector="{target_poses}"
69+
input="{stamped_pose}"
5870
/>
5971
<Action
6072
ID="CreateStampedPose"
6173
orientation_xyzw="0;-1;0;0"
6274
position_xyz="-0.5;0.5;0.8"
75+
stamped_pose="{stamped_pose}"
6376
/>
6477
<Action
6578
ID="AddPoseStampedToVector"
6679
vector="{target_poses}"
80+
input="{stamped_pose}"
6781
/>
6882
<Action
6983
ID="CreateStampedPose"
7084
orientation_xyzw="0;-1;0;0"
7185
position_xyz="-0.75;0.5;0.9"
86+
stamped_pose="{stamped_pose}"
7287
/>
7388
<Action
7489
ID="AddPoseStampedToVector"
7590
vector="{target_poses}"
91+
input="{stamped_pose}"
7692
/>
7793
<Action
7894
ID="CreateStampedPose"
7995
orientation_xyzw="0;-1;0;0"
8096
position_xyz="-1;0.5;1"
97+
stamped_pose="{stamped_pose}"
8198
/>
8299
<Action
83100
ID="AddPoseStampedToVector"
84101
vector="{target_poses}"
102+
input="{stamped_pose}"
85103
/>
86104
</Control>
87105
</BehaviorTree>

‎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/pick_object.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
21
<root BTCPP_format="4" main_tree_to_execute="Pick object">
32
<!--//////////-->
43
<BehaviorTree
@@ -15,6 +14,7 @@
1514
ID="CreateStampedPose"
1615
orientation_xyzw="0;1;0;0"
1716
position_xyz="0.01;.75;0.515"
17+
stamped_pose="{stamped_pose}"
1818
/>
1919
<Action
2020
ID="AddPoseStampedToVector"

‎src/lab_sim/objectives/pick_up_cube.xml‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,13 @@
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="Pick up Cube">
33
<!--//////////-->
44
<BehaviorTree ID="Pick up Cube" _description="" _favorite="false">
55
<Control ID="Sequence" name="TopLevelSequence">
6-
<SubTree ID="Clear Snapshot" _collapsed="true"/>
7-
<SubTree ID="Look at table" _collapsed="true"/>
6+
<SubTree ID="Clear Snapshot" _collapsed="true" />
7+
<SubTree ID="Look at table" _collapsed="true" />
88
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
9-
<SubTree ID="Wrist camera snapshot" _collapsed="true"/>
10-
<SubTree ID="Pick object" _collapsed="false"/>
9+
<SubTree ID="Wrist camera snapshot" _collapsed="true" />
10+
<SubTree ID="Pick object" _collapsed="false" />
1111
</Control>
1212
</BehaviorTree>
1313
</root>

‎src/lab_sim/objectives/place_object.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
ID="CreateStampedPose"
1414
orientation_xyzw="0;1;0;0"
1515
position_xyz="0.01;.75;0.5.15"
16+
stamped_pose="{stamped_pose}"
1617
/>
1718
<Action
1819
ID="AddPoseStampedToVector"

‎src/lab_sim/objectives/plan_and_save_trajectory.xml‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232
/>
3333
<SubTree
3434
ID="Add Waypoints to MTC Task"
35-
_collapsed="true"
35+
_collapsed="false"
3636
mtc_task="{mtc_task}"
3737
waypoint_names="Pick Cube;Place Cube"
3838
joint_group_name="manipulator"
@@ -50,4 +50,7 @@
5050
/>
5151
</Control>
5252
</BehaviorTree>
53+
<TreeNodesModel>
54+
<SubTree ID="Plan and Save Trajectory" />
55+
</TreeNodesModel>
5356
</root>

0 commit comments

Comments
 (0)