Skip to content
Snippets Groups Projects
Commit 13073468 authored by Johannes Mey's avatar Johannes Mey
Browse files

add localization error output

parent 0000368f
Branches
Tags
No related merge requests found
...@@ -31,6 +31,14 @@ using CetiRosToolbox::getPrivateParameter; ...@@ -31,6 +31,14 @@ using CetiRosToolbox::getPrivateParameter;
const std::string CELL_BUNDLE = "CETI_TABLE_ONE"; const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
const double TABLE_HEIGHT = 0.89; const double TABLE_HEIGHT = 0.89;
const double GRID_SIZE = 0.05; 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) geometry_msgs::Point closestGridPoint(geometry_msgs::Point point, double grid_size)
{ {
...@@ -165,6 +173,8 @@ int main(int argc, char** argv) ...@@ -165,6 +173,8 @@ int main(int argc, char** argv)
<< pose.position.z - TABLE_HEIGHT); << pose.position.z - TABLE_HEIGHT);
continue; continue;
} }
double e = distanceToGrid(pose.position, GRID_SIZE);
ROS_ERROR_STREAM("object " << name << " has an error of " << e);
// sanitize pose // sanitize pose
relevant_poses[name] = pose; relevant_poses[name] = pose;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment