Skip to content

added apriltag objectives #60

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
MikeWrock merged 1 commit into from
Dec 18, 2024
Merged

added apriltag objectives #60

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions src/lab_sim/objectives/51mm_apriltag_detection_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
DetectAprilTags:
apriltag_family_name: 36h11
apriltag_size: 0.051
max_hamming: 0
n_threads: 1
quad_decimate: 1
quad_sigma: 0.1
refine_edges: false
z_up: true
46 changes: 46 additions & 0 deletions src/lab_sim/objectives/loop_detect_apriltag.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
<!-- ////////// -->
<BehaviorTree
ID="Loop Detect AprilTag"
_description="Loops finding April Tag markers until failure."
_favorite="false"
>
<Control ID="Sequence">
<!-- Keep executing until failure -->
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence" name="GetDetection">
<Action
ID="LoadObjectiveParameters"
config_file_name="51mm_apriltag_detection_config.yaml"
parameters="{parameters}"
/>
<Action
ID="GetCameraInfo"
topic_name="/wrist_camera/camera_info"
message_out="{camera_info}"
/>
<Action
ID="GetImage"
topic_name="/wrist_camera/color"
message_out="{image}"
/>
<Action
ID="DetectAprilTags"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
/>
</Control>
</Decorator>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Loop Detect AprilTag">
<MetadataFields>
<Metadata subcategory="Perception - 2D Image" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
69 changes: 69 additions & 0 deletions src/lab_sim/objectives/sample_april_tag.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
<!--//////////-->
<BehaviorTree
ID="Sample April Tag"
_description="Samples the pose of an AprilTag and returns the average measurement."
_favorite="false"
_subtreeOnly="true"
>
<Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
<Control ID="Sequence">
<Action
ID="LoadObjectiveParameters"
config_file_name="{apriltag_config}"
parameters="{parameters}"
/>
<Action
ID="GetCameraInfo"
topic_name="/wrist_camera/camera_info"
message_out="{camera_info}"
/>
<Decorator ID="RetryUntilSuccessful" num_attempts="5">
<Control ID="Sequence">
<Action
ID="GetImage"
topic_name="/wrist_camera/color"
message_out="{image}"
/>
<Action
ID="DetectAprilTags"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
/>
<Action
ID="GetDetectionPose"
detections="{detections}"
target_id="{tag_id}"
target_label=""
detection_pose="{detection_pose}"
/>
</Control>
</Decorator>
<Action
ID="AveragePoseStamped"
pose_sample="{detection_pose}"
avg_pose="{avg_pose}"
num_samples="{num_samples}"
run_continuously="false"
/>
</Control>
</Decorator>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Sample April Tag">
<MetadataFields>
<Metadata subcategory="Perception - 2D Image" />
</MetadataFields>
<output_port name="avg_pose" default="{avg_pose}" />
<input_port name="tag_id" default="-1" />
<input_port
name="apriltag_config"
default="51mm_apriltag_detection_config.yaml"
/>
<input_port name="num_samples" default="5" />
</SubTree>
</TreeNodesModel>
</root>
Loading