From 13073468092a365eee17bd75665cba65ea74d9b1 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Fri, 15 Jul 2022 16:27:31 +0200
Subject: [PATCH] add localization error output
---
src/object_locator.cpp | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/src/object_locator.cpp b/src/object_locator.cpp
index ce89302..1071baf 100644
--- a/src/object_locator.cpp
+++ b/src/object_locator.cpp
@@ -31,6 +31,14 @@ using CetiRosToolbox::getPrivateParameter;
const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const double TABLE_HEIGHT = 0.89;
const double GRID_SIZE = 0.05;
+const double MAX_ERROR = 0.02;
+
+double distanceToGrid(geometry_msgs::Point point, double grid_size)
+{
+ double x_error = (std::round(point.x / grid_size)) * grid_size - point.x;
+ double y_error = (std::round(point.y / grid_size)) * grid_size - point.y;
+ return std::sqrt(x_error*x_error + y_error*y_error);
+}
geometry_msgs::Point closestGridPoint(geometry_msgs::Point point, double grid_size)
{
@@ -165,6 +173,8 @@ int main(int argc, char** argv)
<< pose.position.z - TABLE_HEIGHT);
continue;
}
+ double e = distanceToGrid(pose.position, GRID_SIZE);
+ ROS_ERROR_STREAM("object " << name << " has an error of " << e);
// sanitize pose
relevant_poses[name] = pose;
@@ -215,4 +225,4 @@ int main(int argc, char** argv)
}
return 0;
-}
\ No newline at end of file
+}
--
GitLab