How to extract the map matrix after subscribing to /map of map_server?

I need to run Dijkstra's algorithm on this map.

So till now, I've used map_server and got the /map and /map_metadata topics. I'm not quite sure how to proceed further.

This is what I've done so far.

#include < ros/ros.h>

void dijkstra() {

}

int main(int argc, char **argv) {

ros::init(argc, argv, "shortestpath");

ros::Nodehandle n("~");

ros::Subscriber sub = n.subscribe("/map", 1000, dijkstra);

}

/map gives a 1D array so I need to use it and convert it in a 2D matrix.

edit retag close merge delete

Why don't you use amcl, and move_base? What is that you want to do exactly?

( 2019-11-07 01:39:44 -0500 )edit

Sort by ยป oldest newest most voted

Hi Parth2851,

As you mentioned, the map message consists on a 1D array containing all the information.

If you want to convert this to a 2D matrix, you can extract the positions using the map info metadata, where the height, width and resolution of the map are provided.

A simple function to extract a given position (in pixels) of the map, can be like this

int getMapValue(int height, int width, const nav_msgs::OccupancyGrid& map)
{
int position = map.info.width * height + width;        // Finds the position in the data vector.
return map.data[position];                  // Gets the value from the position
}

if you want to extract a position on meters, then you should take into account the resolution of the map, also available on the map info message.

Regards,

more

How about making a new 2D array from the given array, since it would be easier to visualise that way? Is there a downside to that?

int get2Dmap(int height, int width, const nav_msgs::OccupancyGrid& map)
{
int k = 0, a[height][width];
for(int i = 0; i < height; i++)
{
for(int j = 0; j < width; j++)
{
a[i][j] = map[k];
k++;
}
}
return a;
}

( 2019-11-07 08:37:31 -0500 )edit

well, it the code seems right, although I don't know if that's the most efficient way to visualize the map. Usually ROS maps are very large (the default resolution is about 5cm), so the resulting 2D array will be very large. Depending on what's your objective, it may be more effective to work directly with the OccupancyGrids.

( 2019-11-08 08:04:14 -0500 )edit

Yeah, makes sense. Thanks a lot!

( 2019-11-09 01:44:30 -0500 )edit