add localization error output

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
+}