Skip to content

Commit 2e3a141

Browse files
MikeWrockMikeWrock
committed
added apriltag objectives
1 parent 4f40bad commit 2e3a141

File tree

3 files changed

+124
-0
lines changed

3 files changed

+124
-0
lines changed

‎src/lab_sim/objectives/51mm_apriltag_detection_config.yaml‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
DetectAprilTags:
2+
apriltag_family_name: 36h11
3+
apriltag_size: 0.051
4+
max_hamming: 0
5+
n_threads: 1
6+
quad_decimate: 1
7+
quad_sigma: 0.1
8+
refine_edges: false
9+
z_up: true

‎src/lab_sim/objectives/loop_detect_apriltag.xml‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
3+
<!-- ////////// -->
4+
<BehaviorTree
5+
ID="Loop Detect AprilTag"
6+
_description="Loops finding April Tag markers until failure."
7+
_favorite="false"
8+
>
9+
<Control ID="Sequence">
10+
<!-- Keep executing until failure -->
11+
<Decorator ID="KeepRunningUntilFailure">
12+
<Control ID="Sequence" name="GetDetection">
13+
<Action
14+
ID="LoadObjectiveParameters"
15+
config_file_name="51mm_apriltag_detection_config.yaml"
16+
parameters="{parameters}"
17+
/>
18+
<Action
19+
ID="GetCameraInfo"
20+
topic_name="/wrist_camera/camera_info"
21+
message_out="{camera_info}"
22+
/>
23+
<Action
24+
ID="GetImage"
25+
topic_name="/wrist_camera/color"
26+
message_out="{image}"
27+
/>
28+
<Action
29+
ID="DetectAprilTags"
30+
image="{image}"
31+
camera_info="{camera_info}"
32+
parameters="{parameters}"
33+
detections="{detections}"
34+
/>
35+
</Control>
36+
</Decorator>
37+
</Control>
38+
</BehaviorTree>
39+
<TreeNodesModel>
40+
<SubTree ID="Loop Detect AprilTag">
41+
<MetadataFields>
42+
<Metadata subcategory="Perception - 2D Image" />
43+
</MetadataFields>
44+
</SubTree>
45+
</TreeNodesModel>
46+
</root>

‎src/lab_sim/objectives/sample_april_tag.xml‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="Sample April Tag"
6+
_description="Samples the pose of an AprilTag and returns the average measurement."
7+
_favorite="false"
8+
_subtreeOnly="true"
9+
>
10+
<Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
11+
<Control ID="Sequence">
12+
<Action
13+
ID="LoadObjectiveParameters"
14+
config_file_name="{apriltag_config}"
15+
parameters="{parameters}"
16+
/>
17+
<Action
18+
ID="GetCameraInfo"
19+
topic_name="/wrist_camera/camera_info"
20+
message_out="{camera_info}"
21+
/>
22+
<Decorator ID="RetryUntilSuccessful" num_attempts="5">
23+
<Control ID="Sequence">
24+
<Action
25+
ID="GetImage"
26+
topic_name="/wrist_camera/color"
27+
message_out="{image}"
28+
/>
29+
<Action
30+
ID="DetectAprilTags"
31+
image="{image}"
32+
camera_info="{camera_info}"
33+
parameters="{parameters}"
34+
detections="{detections}"
35+
/>
36+
<Action
37+
ID="GetDetectionPose"
38+
detections="{detections}"
39+
target_id="{tag_id}"
40+
target_label=""
41+
detection_pose="{detection_pose}"
42+
/>
43+
</Control>
44+
</Decorator>
45+
<Action
46+
ID="AveragePoseStamped"
47+
pose_sample="{detection_pose}"
48+
avg_pose="{avg_pose}"
49+
num_samples="{num_samples}"
50+
run_continuously="false"
51+
/>
52+
</Control>
53+
</Decorator>
54+
</BehaviorTree>
55+
<TreeNodesModel>
56+
<SubTree ID="Sample April Tag">
57+
<MetadataFields>
58+
<Metadata subcategory="Perception - 2D Image" />
59+
</MetadataFields>
60+
<output_port name="avg_pose" default="{avg_pose}" />
61+
<input_port name="tag_id" default="-1" />
62+
<input_port
63+
name="apriltag_config"
64+
default="51mm_apriltag_detection_config.yaml"
65+
/>
66+
<input_port name="num_samples" default="5" />
67+
</SubTree>
68+
</TreeNodesModel>
69+
</root>

0 commit comments

Comments
 (0)