Ask Your Question

nav2d stage to gridmap transformation

asked 2018-09-08 03:27:19 -0500

alsora gravatar image

updated 2018-09-08 03:30:22 -0500


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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2018-09-08 23:44:44 -0500

alsora gravatar image

I found the solution in the Nav2d code itself.

The navigator sends the goal to Rviz through a marker using the following transformation.

marker.pose.position.x = mCurrentMap.getOriginX() + (((double)goal_x+0.5) * mCurrentMap.getResolution());
marker.pose.position.y = mCurrentMap.getOriginY() + (((double)goal_y+0.5) * mCurrentMap.getResolution());

I only had to apply its inverse.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools


Asked: 2018-09-08 03:27:19 -0500

Seen: 82 times

Last updated: Sep 08 '18