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

Merge branch 'fix/working_on_navigation' of gitlab.uni-koblenz.de:robbie/homer_nav_libs

parents 3b3583c8 ba08eb34
......@@ -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