Combine two OccupancyGrids

edit

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();
    }
}