Commit ba08eb34 authored by Daniel Müller's avatar Daniel Müller
Browse files

Added toMapCoords function for Eigen::Vector2d

parent 1976f18e
......@@ -34,6 +34,14 @@ Eigen::Vector2i toMapCoords(const geometry_msgs::Point& p, const geometry_msgs::
return ret;
}
Eigen::Vector2i toMapCoords(const Eigen::Vector2d& p, const geometry_msgs::Pose& origin,
float resolution)
{
int x_idx = (p.x() - origin.position.x) / resolution + 0.51;
int y_idx = (p.y() - origin.position.y) / resolution + 0.51;
Eigen::Vector2i ret(x_idx, y_idx);
return ret;
}
/**
* @brief Converts a point p in world frame /map to the respective cell position
* in the map
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment