map_loader.cpp

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Brian Gerkey */


#include <stdio.h>
#include <stdlib.h>
#include <libgen.h>
#include <fstream>

#include "ros/ros.h"
#include "ros/console.h"
#include "image_loader.h"
#include "nav_msgs/MapMetaData.h"
#include "yaml-cpp/yaml.h"
#include "tf/transform_datatypes.h"

#include "map_loader.h"

/** Trivial constructor */
MapServer::MapServer(const std::string fname)
{
	std::string slammapfname = "";
	std::string maskingmapfname = "";
	double origin[3];
	int negate;
	double res, occ_th, free_th;
	std::string frame_id;
	frame_id = "map";
	//mapfname = fname + ".pgm";
	//std::ifstream fin((fname + ".yaml").c_str());
	std::ifstream fin(fname.c_str());
	if (fin.fail()) {
		ROS_ERROR("Map_server could not open %s.", fname.c_str());
		exit(-1);
	}
#ifdef HAVE_NEW_YAMLCPP
	YAML::Node doc = YAML::LoadFile(fname);

	try { 
		res = doc["resolution"].as<double>();
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
		exit(-1);
	}
	try { 
		negate = doc["negate"].as<int>(); 
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain a negate tag or it is invalid.");
		exit(-1);
	}
	try { 
		occ_th = doc["occupied_thresh"].as<double>(); 
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
		exit(-1);
	}
	try { 
		free_th = doc["free_thresh"].as<double>(); 
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
		exit(-1);
	}
	try { 
		origin[0] = doc["origin"][0].as<double>(); 
		origin[1] = doc["origin"][1].as<double>(); 
		origin[2] = doc["origin"][2].as<double>(); 
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain an origin tag or it is invalid.");
		exit(-1);
	}
	try {
		slammapfname = doc["image"].as<std::string>();
		// TODO: make this path-handling more robust
		if(slammapfname.size() == 0)
		{
			ROS_ERROR("The image tag cannot be an empty string.");
			exit(-1);
		}
		if(slammapfname[0] != '/')
		{
			// dirname can modify what you pass it
			char* fname_copy = strdup(fname.c_str());
			slammapfname = std::string(dirname(fname_copy)) + '/' + slammapfname;
			free(fname_copy);
		}
	} catch (YAML::InvalidScalar) { 
		ROS_ERROR("The map does not contain an image tag or it is invalid.");
		exit(-1);
	}
	//get masking map image path if available
	if(doc["mask_image"])
	{
		maskingmapfname = doc["mask_image"].as<std::string>();
		// TODO: make this path-handling more robust
		if(maskingmapfname.size() == 0)
		{
			ROS_ERROR("The image tag cannot be an empty string.");
			exit(-1);
		}
		if(maskingmapfname[0] != '/')
		{
			//              // dirname can modify what you pass it
			char* fname_copy = strdup(fname.c_str());
			maskingmapfname = std::string(dirname(fname_copy)) + '/' + maskingmapfname;
			free(fname_copy);
		}
	}

	//get POIs if existent
	if(doc["pois"])
	{
		ROS_INFO_STREAM("Found "  << doc["pois"].size() << " pois");
		for(size_t i = 0; i < doc["pois"].size(); ++i)
		{
			std::string name;
			int type;
			float posX;
			float posY;
			float theta;
			std::string remarks;
			name = doc["pois"][i]["name"].as<std::string>() ;
			type = doc["pois"][i]["type"].as<int>() ;
			posX = doc["pois"][i]["x"].as<double>() ;
			posY = doc["pois"][i]["y"].as<double>() ;
			theta =	doc["pois"][i]["theta"].as<double>() ;
			remarks = doc["pois"][i]["remarks"].as<std::string>("-") ;

			homer_mapnav_msgs::PointOfInterest poi;
			poi.name = name;
			poi.type = type;
			poi.pose.position.x = posX;
			poi.pose.position.y = posY;
			poi.pose.position.z = 0.0;
			poi.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
			poi.remarks = remarks;

			poiList.push_back(poi);
		}
	}
#else
#warning "yaml-cpp version < 0.5.0 does not work"
	// For Ubuntu Saucy to work, this would require a yaml-cpp
	// 0.3.0 compatible implementation.  Since we don't use Saucy,
	// we just let it fail.
#endif
	ROS_INFO("Loading SLAM map from image \"%s\"", slammapfname.c_str());
	map_server::loadMapFromFile(&m_SLAMMap,slammapfname.c_str(),res,negate,occ_th,free_th, origin);
	m_SLAMMap.info.map_load_time = ros::Time::now();
	m_SLAMMap.header.frame_id = frame_id;
	m_SLAMMap.header.stamp = ros::Time::now();
	ROS_INFO("Read a %d X %d SLAM map @ %.3lf m/cell",
			m_SLAMMap.info.width,
			m_SLAMMap.info.height,
			m_SLAMMap.info.resolution);

	if(maskingmapfname != "")
	{
		ROS_INFO("Loading masking map from image \"%s\"", maskingmapfname.c_str());
		map_server::loadMapFromFile(&m_MaskingMap,maskingmapfname.c_str(),res,negate,occ_th,free_th, origin);
		m_MaskingMap.info.map_load_time = ros::Time::now();
		m_MaskingMap.header.frame_id = frame_id;
		m_MaskingMap.header.stamp = ros::Time::now();
		ROS_INFO("Read a %d X %d masking map @ %.3lf m/cell",
				m_MaskingMap.info.width,
				m_MaskingMap.info.height,
				m_MaskingMap.info.resolution);
	}

}

nav_msgs::OccupancyGrid MapServer::getSLAMMap()
{
	return m_SLAMMap;
}

nav_msgs::OccupancyGrid MapServer::getMaskingMap()
{
	return m_MaskingMap;
}

std::vector<homer_mapnav_msgs::PointOfInterest> MapServer::getPois()
{
	return poiList;
}