Ask Your Question
2

Combine two OccupancyGrids

asked 2013-09-08 20:09:45 -0600

Johannes gravatar image

updated 2013-09-12 00:40:28 -0600

Hello, I would like to combine two Occupancy Grids (one comming from a map_server and one coming from my own node that uses sensor data to crate a map). In a previous posting I was reading about occupnacy_grid_utils and the fuction occupancy_grid_utils::combineGrids which works fine for combining two OccupancyGrids. The only problem I'm facing is that the Origin of the two grids is not the same. On Grid has its origin at Odom, the other at Map. From a localizer I do know the transformation between odom and map, but I do not know how to tell combineGrids(..) to consider this transformation. I thougt about to change the grid->info->origin, but from my subscriber callback I get a constant pointer and I can't change the origin of the grid. What do I have to do so that the grids get aligned to each other.

I think the solution will be trival, but I can't figure it out.

My code looks as follows:

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include <occupancy_grid_utils/combine_grids.h>

/// This file merges two occupancy grids

nav_msgs::OccupancyGrid::ConstPtr map1;
nav_msgs::OccupancyGrid::ConstPtr map2;
bool newMap1 = false;
bool newMap2 = false;
bool eachMapReceivedOnce = false;

void map1Callback(const nav_msgs::OccupancyGrid::ConstPtr &msg) {
    map1 = msg;
    newMap1 = true;
}

void map2Callback(const nav_msgs::OccupancyGrid::ConstPtr &msg) {
    map2 = msg;
    newMap2 = true;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "mergeMap");

    ros::NodeHandle n;
    ros::Publisher map_pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);
    ros::Subscriber map1_sub = n.subscribe("/map_1", 10, map1Callback);
    ros::Subscriber map2_sub = n.subscribe("/map_2", 10, map2Callback);
    tf::TransformListener listener;

    ros::Rate r(5.0);

    while(n.ok()){
        if (newMap1 && newMap2) {
            eachMapReceivedOnce = true;
        }

        if(eachMapReceivedOnce && (newMap1 || newMap2)){
            std::vector<nav_msgs::OccupancyGrid::ConstPtr> grids(2);
            nav_msgs::OccupancyGrid::ConstPtr mapOut;

            //TODO, move origin of map to align them

            grids[0] = map1;
            grids[1] = map2;

            mapOut = occupancy_grid_utils::combineGrids(grids, 0.05);
            map_pub.publish(mapOut);

            newMap1 = false;
            newMap2 = false;
        }

        ros::spinOnce();
        r.sleep();
    }
}
edit retag flag offensive close merge delete

Comments

@Johannes Could you save the merged map created on /map topic using map_saver ? Because I could get merged map with one node (map_server - already saved map) and another map through SLAM , I could view in rViz on /map topic but unable to save through map_saver. Any suggestions ? Thanks, Devasena

Devasena Inupakutika gravatar imageDevasena Inupakutika ( 2013-09-10 09:45:47 -0600 )edit

Sorry, I did not try this.

Johannes gravatar imageJohannes ( 2013-09-12 00:38:33 -0600 )edit

3 Answers

Sort by ยป oldest newest most voted
1

answered 2013-09-09 01:11:16 -0600

Jbot gravatar image

Hi,

You can try to use this package which combine 2 maps in 1 : http://wiki.ros.org/mapstitch

edit flag offensive delete link more

Comments

I'm going to check this, looks quite good. Thx

Johannes gravatar imageJohannes ( 2013-09-09 21:08:12 -0600 )edit
0

answered 2013-12-04 02:44:45 -0600

yz gravatar image

updated 2013-12-04 02:46:06 -0600

Hi,

I think a very simple solution is that just assume the relative positions of these two maps are known. For example,

int delta_x = -20, delta_y = 20;

Then you can simply combine the two maps by a few lines of code instead of using:

ccupancy_grid_utils::combineGrids

edit flag offensive delete link more

Comments

Would you happen to know an efficient way to do this?

aravindp gravatar imagearavindp ( 2016-09-29 17:49:21 -0600 )edit
0

answered 2014-02-08 18:22:34 -0600

You can use costmap_node from costmap_2d package. This node recieve a static map (published by map_server) and allows sensor data to create the occupancy grid (Laser or PointCloud).

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

2 followers

Stats

Asked: 2013-09-08 20:09:45 -0600

Seen: 1,906 times

Last updated: Feb 08 '14