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
No related tags found
No related merge requests found
......@@ -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
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment