nav2d stage to gridmap transformation

edit

Hi,

I'm using nav2d in ros kinetic. The configuration is the one of tutorial3.launch.

During the navigation I printed the start and goal coordinates used by the exploration algorithm (NearestFrontierPlanner). However I am not able to establish the relation between these values and the real robot position (provided by stage).

My goal would be to understand this relation in order to send navigation goals to the robot. From the stage coordinates I could obtain the gridmap ones and finally the cell index to send as goal to the navigator.

This is what I tried:

int compute_goal(GridMap* map, unsigned_int &goal, float stage_x, float stage_y)
{
    double resolution = map->getResolution();

    auto grid_x = stage_x / resolution;
    auto grid_y = stage_y / resolution;

    unsigned int index;
    map->getIndex(grid_x, grid_y, index);

    goal = index;
    return EXPL_TARGET_SET;
}

The outcome is wrong. The navigator always says that the point is not reachable.