CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_detect: check for duplicates in marker transforms, send all transforms in one message

Browse Source
This commit is contained in:
Oleg Kalachev
2022-09-14 12:55:27 +03:00
parent dd0dd6b5c1
commit 55f8f4fa1a
1 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
aruco_pose/src/aruco_detect.cpp
View File

@@ -187,6 +187,8 @@ private:
array_.markers.reserve(ids.size());
aruco_pose::Marker marker;
vector<geometry_msgs::TransformStamped> transforms;
transforms.reserve(ids.size());
geometry_msgs::TransformStamped transform;
transform.header.stamp = msg->header.stamp;
transform.header.frame_id = msg->header.frame_id;
@@ -204,20 +206,33 @@ private:
snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
}
// TODO: check IDs are unique
if (send_tf_) {
transform.child_frame_id = getChildFrameId(ids[i]);
// check if such static transform is in the map
if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
br_->sendTransform(transform);
// check if a markers with that id is already added
bool send = true;
for (auto &t : transforms) {
if (t.child_frame_id == transform.child_frame_id) {
send = false;
break;
}
}
if (send) {
transform.transform.rotation = marker.pose.orientation;
fillTranslation(transform.transform.translation, tvecs[i]);
transforms.push_back(transform);
}
}
}
}
array_.markers.push_back(marker);
}
if (send_tf_) {
br_->sendTransform(transforms);
}
}
markers_pub_.publish(array_);