map_server_node.cpp

Go to the documentation of this file.

/***********************************************************************************
 * Revised BSD License                                                             *
 * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at>                    *
 * 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 Vienna University of Technology 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 Markus Bader 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.                    *
 ***********************************************************************************/

#include <map_server_node.h>

#include <mrpt_bridge/map.h>

#include <mrpt/system/filesystem.h> // ASSERT_FILE_EXISTS_()
#include <mrpt/slam/CMultiMetricMap.h>
#include <mrpt/slam/COccupancyGridMap2D.h>
#include <mrpt/utils/CConfigFile.h>


MapServer::MapServer(ros::NodeHandle &n)
    : n_(n)
    , n_param_("~")
    , loop_count_(0)
    , frequency_(0)
    , debug_(true) {
}

MapServer::~MapServer() {
}

void MapServer::init() {
    std::string ini_file;
    std::string map_file;
    n_param_.param<bool>("debug", debug_, true);
    ROS_INFO("debug: %s", (debug_?"true":"false"));
    n_param_.param<std::string>("ini_file", ini_file, "map.ini");
    ROS_INFO("ini_file: %s", ini_file.c_str());
    n_param_.param<std::string>("map_file", map_file, "map.simplemap");
    ROS_INFO("map_file: %s", map_file.c_str());
    n_param_.param<std::string>("frame_id", resp_ros_.map.header.frame_id, "map");
    ROS_INFO("frame_id: %s", resp_ros_.map.header.frame_id.c_str());
    n_param_.param<double>("frequency", frequency_, 0.1);
    ROS_INFO("frequency: %f", frequency_);

    pub_map_ros_ = n_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
    pub_metadata_= n_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
    service_map_ = n_.advertiseService("static_map", &MapServer::mapCallback, this);

    if(!mrpt::system::fileExists(ini_file)){
      ROS_ERROR("ini_file: %s does not exit", ini_file.c_str());
    }
    if(!mrpt::system::fileExists(map_file)){
      ROS_ERROR("map_file: %s does not exit", map_file.c_str());
    }
    ASSERT_FILE_EXISTS_(ini_file);
    mrpt::utils::CConfigFile config_file;
    config_file.setFileName(ini_file);
    metric_map_ = boost::shared_ptr<mrpt::slam::CMultiMetricMap>(new  mrpt::slam::CMultiMetricMap);
    mrpt_bridge::MapHdl::loadMap(*metric_map_, config_file, map_file, "metricMap", debug_);
    const mrpt::slam::COccupancyGridMap2D &grid = *metric_map_->m_gridMaps[0];
    if(debug_) printf("gridMap[0]:  %i x %i @ %4.3fm/p, %4.3f, %4.3f, %4.3f, %4.3f\n",
                      grid.getSizeX(), grid.getSizeY(), grid.getResolution(),
                      grid.getXMin(), grid.getYMin(),
                      grid.getXMax(), grid.getYMax());

    mrpt_bridge::convert(*metric_map_->m_gridMaps[0], resp_ros_.map);
    if(debug_) printf("msg:         %i x %i @ %4.3fm/p, %4.3f, %4.3f, %4.3f, %4.3f\n",
                      resp_ros_.map.info.width, resp_ros_.map.info.height, resp_ros_.map.info.resolution,
                      resp_ros_.map.info.origin.position.x, resp_ros_.map.info.origin.position.y,
                      resp_ros_.map.info.width*resp_ros_.map.info.resolution+resp_ros_.map.info.origin.position.x,
                      resp_ros_.map.info.height*resp_ros_.map.info.resolution+resp_ros_.map.info.origin.position.y);
}

bool MapServer::mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res )
{
  ROS_INFO("mapCallback: service requested!\n");
  res = resp_ros_;
  return true;
}

void MapServer::publishMap () {
    resp_ros_.map.header.stamp = ros::Time::now();
    resp_ros_.map.header.seq = loop_count_;
    if(pub_map_ros_.getNumSubscribers() > 0){
        pub_map_ros_.publish(resp_ros_.map );
    }
    if(pub_metadata_.getNumSubscribers() > 0){
        pub_metadata_.publish( resp_ros_.map.info );
    }
}

void MapServer::loop() {
    if(frequency_ > 0){
        ros::Rate rate(frequency_);
        for(loop_count_ = 0; ros::ok(); loop_count_++) {
          publishMap ();
          ros::spinOnce();
          rate.sleep();
        }
    } else {
        publishMap ();
        ros::spin();
    }
}

int main(int argc, char **argv)
{
    // Initialize ROS
    ros::init(argc, argv, "mrpt_map_server");
    ros::NodeHandle node;
    MapServer my_node(node);
    my_node.init();
    my_node.loop();
    return 0;
}