#include <stdio.h> #include <stdlib.h> #include <sstream> #include <string> #include <vector> #include "MapManager.h" #include <nav_msgs/OccupancyGrid.h> #include <homer_mapnav_msgs/ModifyMap.h> #include <homer_mapnav_msgs/MapLayers.h> #include <tools/tools.h> MapManager::MapManager(ros::NodeHandle* nh) { m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1); //enable all map layers m_MapVisibility[homer_mapnav_msgs::MapLayers::SLAM_LAYER] = true; m_MapVisibility[homer_mapnav_msgs::MapLayers::KINECT_LAYER] = true; m_MapVisibility[homer_mapnav_msgs::MapLayers::SICK_LAYER] = true; m_MapVisibility[homer_mapnav_msgs::MapLayers::MASKING_LAYER] = true; m_Height = -1; m_Width = -1; m_Resolution = -1; } MapManager::~MapManager() { } void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer) { m_MapLayers[type] = layer; //if slam map update map sizes if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER) { m_Height = layer->info.height; m_Width = layer->info.width; m_Resolution = layer->info.resolution; m_Origin = layer->info.origin; sendMergedMap(); } } void MapManager::clearMapLayers() { m_MapLayers.clear(); } void MapManager::toggleMapVisibility(int type, bool state) { ROS_INFO_STREAM("MapManager: " << type << ": " << state); m_MapVisibility[type] = state; } nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) { if(m_MapLayers.find(type) == m_MapLayers.end()) return nav_msgs::OccupancyGrid::ConstPtr(); return m_MapLayers[type]; } /** * Sends the SLAM map (OccupancyGrid) and (if available and enabled) other merged map layers to the gui node * */ void MapManager::sendMergedMap() { if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) == m_MapLayers.end() ) { ROS_ERROR_STREAM( "SLAM map is missing!" ); return; } nav_msgs::OccupancyGrid mergedMap( *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]) ); //apply kinect map if enabled if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::KINECT_LAYER) != m_MapLayers.end() && m_MapVisibility[homer_mapnav_msgs::MapLayers::KINECT_LAYER]) { nav_msgs::OccupancyGridConstPtr &kinectMap = m_MapLayers[homer_mapnav_msgs::MapLayers::KINECT_LAYER]; for ( int y = 0; y < kinectMap->info.height; y++ ) { for ( int x = 0; x < kinectMap->info.width; x++ ) { int i = x + y * kinectMap->info.width; //if cell is occupied by kinect obstacle merge cell with merged map if(kinectMap->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED) { Eigen::Vector2i point(x,y); geometry_msgs::Point tmp = map_tools::fromMapCoords( point ,kinectMap->info.origin, kinectMap->info.resolution); point = map_tools::toMapCoords(tmp , mergedMap.info.origin, mergedMap.info.resolution); int k = point.y() * mergedMap.info.width + point.x(); mergedMap.data[k] = homer_mapnav_msgs::ModifyMap::KINECT; } } } } //apply SICK map if enabled if(m_MapLayers.find(homer_mapnav_msgs::MapLayers::SICK_LAYER) != m_MapLayers.end() && m_MapVisibility[homer_mapnav_msgs::MapLayers::SICK_LAYER]) { nav_msgs::OccupancyGridConstPtr &sickMap = m_MapLayers[homer_mapnav_msgs::MapLayers::SICK_LAYER]; if ( ( sickMap->info.width == mergedMap.info.width ) && ( sickMap->info.height == mergedMap.info.height ) ) { for ( int i=0; i<mergedMap.data.size(); i++ ) { //if cell is occupied by kinect obstacle merge cell with merged map if(sickMap->data[i] == homer_mapnav_msgs::ModifyMap::BLOCKED) { mergedMap.data[i] = homer_mapnav_msgs::ModifyMap::BLOCKED; } } } else { ROS_ERROR_STREAM( "Size mismatch between SLAM map and SICK map!" ); } } //apply masking map if enabled if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::MASKING_LAYER) != m_MapLayers.end() && m_MapVisibility[homer_mapnav_msgs::MapLayers::MASKING_LAYER]) { nav_msgs::OccupancyGridConstPtr &maskingMap = m_MapLayers[homer_mapnav_msgs::MapLayers::MASKING_LAYER]; if ( ( maskingMap->info.width == mergedMap.info.width ) && ( maskingMap->info.height == mergedMap.info.height ) ) { for ( int i=0; i<mergedMap.data.size(); i++ ) { //if cell should be masked apply masking on merged map if(maskingMap->data[i] != homer_mapnav_msgs::ModifyMap::NOT_MASKED) { mergedMap.data[i] = maskingMap->data[i]; } } } else { ROS_ERROR_STREAM( "Size mismatch between SLAM map (" << mergedMap.info.width << "x" << mergedMap.info.height << ") and masking map (" << maskingMap->info.width << "x" << maskingMap->info.height << ")!" ); } } m_MapPublisher.publish(mergedMap); ROS_DEBUG_STREAM("Publishing map"); }