Skip to content

Commit 79a0a0d

Browse files
mariopratsmarioprats
authored
Merge pull request #265 from PickNikRobotics/update-detect-apriltags-ports
Update DetectApriltags behavior ports
2 parents 26eaff1 + 0bd7664 commit 79a0a0d

13 files changed

+77
-171
lines changed

‎src/lab_sim/objectives/51mm_apriltag_detection_config.yaml

Lines changed: 0 additions & 9 deletions
This file was deleted.

‎src/lab_sim/objectives/apriltag_detection_config.yaml

Lines changed: 0 additions & 9 deletions
This file was deleted.

‎src/lab_sim/objectives/apriltag_pick_object.xml

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,6 @@
1313
<Control ID="Sequence" name="Setup">
1414
<SubTree ID="Open Gripper" />
1515
</Control>
16-
<Action
17-
ID="LoadObjectiveParameters"
18-
config_file_name="apriltag_detection_config.yaml"
19-
parameters="{parameters}"
20-
/>
2116
<Action
2217
ID="GetCameraInfo"
2318
topic_name="/wrist_camera/camera_info"
@@ -34,7 +29,6 @@
3429
ID="Get AprilTag Pose from Image"
3530
image="{image}"
3631
camera_info="{camera_info}"
37-
parameters="{parameters}"
3832
_collapsed="true"
3933
detection_pose="{detection_pose}"
4034
/>
@@ -55,11 +49,16 @@
5549
solution="{move_to_pose_solution}"
5650
goal_duration_tolerance="-1.000000"
5751
/>
52+
<Action
53+
ID="GetImage"
54+
topic_name="/wrist_camera/color"
55+
message_out="{image}"
56+
timeout_sec="5.000000"
57+
/>
5858
<SubTree
5959
ID="Get AprilTag Pose from Image"
6060
image="{image}"
6161
camera_info="{camera_info}"
62-
parameters="{parameters}"
6362
_collapsed="true"
6463
detection_pose="{detection_pose}"
6564
/>
@@ -80,6 +79,8 @@
8079
_collapsed="true"
8180
grasp_pose="{object_pose}"
8281
grasp_poses="{grasp_poses}"
82+
approach_distance="0.1"
83+
retract_xyz="0;0;-0.1"
8384
/>
8485
</Control>
8586
</BehaviorTree>

‎src/lab_sim/objectives/get_april_tag_pose_from_image.xml

Lines changed: 12 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
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="Get AprilTag Pose from Image">
33
<BehaviorTree
44
ID="Get AprilTag Pose from Image"
@@ -10,21 +10,15 @@
1010
ID="DetectAprilTags"
1111
image="{image}"
1212
camera_info="{camera_info}"
13-
parameters="{parameters}"
14-
detections="{detections}"
15-
/>
16-
<Action
17-
ID="GetImage"
18-
topic_name="/wrist_camera/color"
19-
message_out="{image}"
20-
timeout_sec="5.000000"
21-
/>
22-
<Action
23-
ID="DetectAprilTags"
24-
image="{image}"
25-
camera_info="{camera_info}"
26-
parameters="{parameters}"
2713
detections="{detections}"
14+
apriltag_family_name="36h11"
15+
max_hamming="0"
16+
n_threads="1"
17+
quad_decimate="2.0"
18+
quad_sigma="0.0"
19+
refine_edges="true"
20+
tag_size="0.04"
21+
z_up="false"
2822
/>
2923
<Action
3024
ID="GetDetectionPose"
@@ -42,14 +36,13 @@
4236
</BehaviorTree>
4337
<TreeNodesModel>
4438
<SubTree ID="Get AprilTag Pose from Image">
45-
<input_port name="camera_info" default="{camera_info}" />
46-
<output_port name="detection_pose" default="{detection_pose}" />
47-
<input_port name="image" default="{image}" />
48-
<input_port name="parameters" default="{parameters}" />
4939
<MetadataFields>
5040
<Metadata subcategory="Perception - 2D Image" />
5141
<Metadata runnable="false" />
5242
</MetadataFields>
43+
<input_port name="camera_info" default="{camera_info}" />
44+
<output_port name="detection_pose" default="{detection_pose}" />
45+
<input_port name="image" default="{image}" />
5346
</SubTree>
5447
</TreeNodesModel>
5548
</root>

‎src/lab_sim/objectives/loop_detect_apriltag.xml

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
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="Loop Detect AprilTag">
33
<!--//////////-->
44
<BehaviorTree
@@ -10,11 +10,6 @@
1010
<!--Keep executing until failure-->
1111
<Decorator ID="KeepRunningUntilFailure">
1212
<Control ID="Sequence" name="GetDetection">
13-
<Action
14-
ID="LoadObjectiveParameters"
15-
config_file_name="51mm_apriltag_detection_config.yaml"
16-
parameters="{parameters}"
17-
/>
1813
<Action
1914
ID="GetCameraInfo"
2015
topic_name="/wrist_camera/camera_info"
@@ -31,8 +26,15 @@
3126
ID="DetectAprilTags"
3227
image="{image}"
3328
camera_info="{camera_info}"
34-
parameters="{parameters}"
3529
detections="{detections}"
30+
apriltag_family_name="36h11"
31+
max_hamming="0"
32+
n_threads="1"
33+
quad_decimate="2.0"
34+
quad_sigma="0.0"
35+
refine_edges="true"
36+
z_up="true"
37+
tag_size="0.051"
3638
/>
3739
</Control>
3840
</Decorator>

‎src/lab_sim/objectives/pick_april_tag_object_with_approval.xml

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,6 @@
1616
<Control ID="Sequence" name="Setup">
1717
<SubTree ID="Open Gripper" />
1818
</Control>
19-
<Action
20-
ID="LoadObjectiveParameters"
21-
config_file_name="apriltag_detection_config.yaml"
22-
parameters="{parameters}"
23-
/>
2419
<Action
2520
ID="GetCameraInfo"
2621
topic_name="/wrist_camera/camera_info"
@@ -58,6 +53,12 @@
5853
solution="{move_to_pose_solution}"
5954
goal_duration_tolerance="-1.000000"
6055
/>
56+
<Action
57+
ID="GetImage"
58+
topic_name="/wrist_camera/color"
59+
message_out="{image}"
60+
timeout_sec="5.000000"
61+
/>
6162
<SubTree
6263
ID="Get AprilTag Pose from Image"
6364
image="{image}"
@@ -82,6 +83,8 @@
8283
ID="Pick from Pose With Approval"
8384
_collapsed="false"
8485
grasp_poses="{grasp_poses}"
86+
approach_distance="0.1"
87+
retract_xyz="0;0;-0.1"
8588
/>
8689
</Control>
8790
</BehaviorTree>

‎src/lab_sim/objectives/sample_april_tag.xml

Lines changed: 0 additions & 68 deletions
This file was deleted.

‎src/moveit_pro_kinova_configs/kinova_sim/objectives/sample_april_tag.xml

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,6 @@
88
>
99
<Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
1010
<Control ID="Sequence">
11-
<Action
12-
ID="LoadObjectiveParameters"
13-
config_file_name="{apriltag_config}"
14-
parameters="{parameters}"
15-
/>
1611
<Action
1712
ID="GetCameraInfo"
1813
topic_name="/wrist_camera/camera_info"
@@ -29,8 +24,15 @@
2924
ID="DetectAprilTags"
3025
image="{image}"
3126
camera_info="{camera_info}"
32-
parameters="{parameters}"
3327
detections="{detections}"
28+
apriltag_family_name="36h11"
29+
max_hamming="0"
30+
n_threads="1"
31+
quad_decimate="2.0"
32+
quad_sigma="0.0"
33+
refine_edges="true"
34+
tag_size="0.051"
35+
z_up="true"
3436
/>
3537
<Action
3638
ID="GetDetectionPose"

‎src/moveit_pro_kinova_configs/space_satellite_sim/objectives/100mm_apriltag_detection_config.yaml

Lines changed: 0 additions & 9 deletions
This file was deleted.

‎src/moveit_pro_kinova_configs/space_satellite_sim/objectives/sample_april_tag.xml

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,6 @@
88
>
99
<Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
1010
<Control ID="Sequence">
11-
<Action
12-
ID="LoadObjectiveParameters"
13-
config_file_name="{apriltag_config}"
14-
parameters="{parameters}"
15-
/>
1611
<Action
1712
ID="GetCameraInfo"
1813
topic_name="/wrist_camera/camera_info"
@@ -29,8 +24,15 @@
2924
ID="DetectAprilTags"
3025
image="{image}"
3126
camera_info="{camera_info}"
32-
parameters="{parameters}"
3327
detections="{detections}"
28+
apriltag_family_name="36h11"
29+
max_hamming="0"
30+
n_threads="1"
31+
quad_decimate="2.0"
32+
quad_sigma="0.0"
33+
refine_edges="true"
34+
tag_size="0.1"
35+
z_up="true"
3436
/>
3537
<Action
3638
ID="GetDetectionPose"

‎src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/51mm_apriltag_detection_config.yaml

Lines changed: 0 additions & 9 deletions
This file was deleted.

‎src/moveit_pro_ur_configs/picknik_ur_site_config/objectives/loop_detect_apriltag.xml

Lines changed: 16 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,36 +1,40 @@
11
<?xml version="1.0" encoding="UTF-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
3-
<!-- ////////// -->
3+
<!--//////////-->
44
<BehaviorTree
55
ID="Loop Detect AprilTag"
6-
_description="Loops finding April Tag markers until failure."
6+
_description="Loops finding April Tag markers."
77
_favorite="false"
88
>
99
<Control ID="Sequence">
10-
<!-- Keep executing until failure -->
10+
<!--Keep executing until failure-->
1111
<Decorator ID="KeepRunningUntilFailure">
1212
<Control ID="Sequence" name="GetDetection">
13-
<Action
14-
ID="LoadObjectiveParameters"
15-
config_file_name="51mm_apriltag_detection_config.yaml"
16-
parameters="{parameters}"
17-
/>
1813
<Action
1914
ID="GetCameraInfo"
20-
topic_name="/wrist_mounted_camera/color/camera_info"
15+
topic_name="/wrist_camera/camera_info"
2116
message_out="{camera_info}"
17+
timeout_sec="5.000000"
2218
/>
2319
<Action
2420
ID="GetImage"
25-
topic_name="/wrist_mounted_camera/color/image_raw"
21+
topic_name="/wrist_camera/color"
2622
message_out="{image}"
23+
timeout_sec="5.000000"
2724
/>
2825
<Action
2926
ID="DetectAprilTags"
3027
image="{image}"
3128
camera_info="{camera_info}"
32-
parameters="{parameters}"
3329
detections="{detections}"
30+
apriltag_family_name="36h11"
31+
max_hamming="0"
32+
n_threads="1"
33+
quad_decimate="2.0"
34+
quad_sigma="0.0"
35+
refine_edges="true"
36+
z_up="true"
37+
tag_size="0.051"
3438
/>
3539
</Control>
3640
</Decorator>
@@ -40,6 +44,7 @@
4044
<SubTree ID="Loop Detect AprilTag">
4145
<MetadataFields>
4246
<Metadata subcategory="Application - Basic Examples" />
47+
<Metadata runnable="true" />
4348
</MetadataFields>
4449
</SubTree>
4550
</TreeNodesModel>

0 commit comments

Comments
 (0)