i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap: Added warning when frame_id of individual tags is different from the msg array.

Browse Source
This commit is contained in:
matlabbe
2020-08-03 12:20:41 -04:00
parent 57dc95c9d7
commit 56be5dca06
1 changed files with 22 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+22
View File
@@ -2219,10 +2219,32 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
if(!tagDetections.detections[i].pose.header.frame_id.empty()) if(!tagDetections.detections[i].pose.header.frame_id.empty())
{ {
p.header.frame_id = tagDetections.detections[i].pose.header.frame_id; p.header.frame_id = tagDetections.detections[i].pose.header.frame_id;
static bool warned = false;
if(!warned &&
!tagDetections.header.frame_id.empty() &&
tagDetections.detections[i].pose.header.frame_id.compare(tagDetections.header.frame_id)!=0)
{
NODELET_WARN("frame_id set for individual tag detections (%s) doesn't match the frame_id of the message (%s), "
"the resulting pose of the tag may be wrong. This message is only printed once.",
tagDetections.detections[i].pose.header.frame_id.c_str(), tagDetections.header.frame_id.c_str());
warned = true;
}
} }
if(!tagDetections.detections[i].pose.header.stamp.isZero()) if(!tagDetections.detections[i].pose.header.stamp.isZero())
{ {
p.header.stamp = tagDetections.detections[i].pose.header.stamp; p.header.stamp = tagDetections.detections[i].pose.header.stamp;
static bool warned = false;
if(!warned &&
!tagDetections.header.stamp.isZero() &&
tagDetections.detections[i].pose.header.stamp != tagDetections.header.stamp)
{
NODELET_WARN("stamp set for individual tag detections (%f) doesn't match the stamp of the message (%f), "
"the resulting pose of the tag may be wrongly interpolated. This message is only printed once.",
tagDetections.detections[i].pose.header.stamp.toSec(), tagDetections.header.stamp.toSec());
warned = true;
}
} }
uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p)); uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p));
} }