0
$\begingroup$

Rosanswers logo

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

Originally posted by Johannes on ROS Answers with karma: 75 on 2013-09-08

Post score: 2


Original comments

Comment by Devasena Inupakutika on 2013-09-10:
@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

Comment by Johannes on 2013-09-12:
Sorry, I did not try this.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi,

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


Originally posted by Jbot with karma: 429 on 2013-09-09

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Johannes on 2013-09-09:
I'm going to check this, looks quite good. Thx

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.