map_server_node.cpp
Go to the documentation of this file.
00001 /***********************************************************************************
00002  * Revised BSD License                                                             *
00003  * Copyright (c) 2014, Markus Bader <markus.bader@tuwien.ac.at>                    *
00004  * All rights reserved.                                                            *
00005  *                                                                                 *
00006  * Redistribution and use in source and binary forms, with or without              *
00007  * modification, are permitted provided that the following conditions are met:     *
00008  *     * Redistributions of source code must retain the above copyright            *
00009  *       notice, this list of conditions and the following disclaimer.             *
00010  *     * Redistributions in binary form must reproduce the above copyright         *
00011  *       notice, this list of conditions and the following disclaimer in the       *
00012  *       documentation and/or other materials provided with the distribution.      *
00013  *     * Neither the name of the Vienna University of Technology nor the           *
00014  *       names of its contributors may be used to endorse or promote products      *
00015  *       derived from this software without specific prior written permission.     *
00016  *                                                                                 *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND *
00018  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED   *
00019  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE          *
00020  * DISCLAIMED. IN NO EVENT SHALL Markus Bader BE LIABLE FOR ANY                    *
00021  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES      *
00022  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;    *
00023  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND     *
00024  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      *
00025  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS   *
00026  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                    *
00027  ***********************************************************************************/
00028 
00029 #include <map_server_node.h>
00030 
00031 #include <mrpt_bridge/map.h>
00032 
00033 #include <mrpt/system/filesystem.h> // ASSERT_FILE_EXISTS_()
00034 #include <mrpt/slam/CMultiMetricMap.h>
00035 #include <mrpt/slam/COccupancyGridMap2D.h>
00036 #include <mrpt/utils/CConfigFile.h>
00037 
00038 
00039 MapServer::MapServer(ros::NodeHandle &n)
00040     : n_(n)
00041     , n_param_("~")
00042     , loop_count_(0)
00043     , frequency_(0)
00044     , debug_(true) {
00045 }
00046 
00047 MapServer::~MapServer() {
00048 }
00049 
00050 void MapServer::init() {
00051     std::string ini_file;
00052     std::string map_file;
00053     n_param_.param<bool>("debug", debug_, true);
00054     ROS_INFO("debug: %s", (debug_?"true":"false"));
00055     n_param_.param<std::string>("ini_file", ini_file, "map.ini");
00056     ROS_INFO("ini_file: %s", ini_file.c_str());
00057     n_param_.param<std::string>("map_file", map_file, "map.simplemap");
00058     ROS_INFO("map_file: %s", map_file.c_str());
00059     n_param_.param<std::string>("frame_id", resp_ros_.map.header.frame_id, "map");
00060     ROS_INFO("frame_id: %s", resp_ros_.map.header.frame_id.c_str());
00061     n_param_.param<double>("frequency", frequency_, 0.1);
00062     ROS_INFO("frequency: %f", frequency_);
00063 
00064     pub_map_ros_ = n_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
00065     pub_metadata_= n_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
00066     service_map_ = n_.advertiseService("static_map", &MapServer::mapCallback, this);
00067 
00068     if(!mrpt::system::fileExists(ini_file)){
00069       ROS_ERROR("ini_file: %s does not exit", ini_file.c_str());
00070     }
00071     if(!mrpt::system::fileExists(map_file)){
00072       ROS_ERROR("map_file: %s does not exit", map_file.c_str());
00073     }
00074     ASSERT_FILE_EXISTS_(ini_file);
00075     mrpt::utils::CConfigFile config_file;
00076     config_file.setFileName(ini_file);
00077     metric_map_ = boost::shared_ptr<mrpt::slam::CMultiMetricMap>(new  mrpt::slam::CMultiMetricMap);
00078     mrpt_bridge::MapHdl::loadMap(*metric_map_, config_file, map_file, "metricMap", debug_);
00079     const mrpt::slam::COccupancyGridMap2D &grid = *metric_map_->m_gridMaps[0];
00080     if(debug_) printf("gridMap[0]:  %i x %i @ %4.3fm/p, %4.3f, %4.3f, %4.3f, %4.3f\n",
00081                       grid.getSizeX(), grid.getSizeY(), grid.getResolution(),
00082                       grid.getXMin(), grid.getYMin(),
00083                       grid.getXMax(), grid.getYMax());
00084 
00085     mrpt_bridge::convert(*metric_map_->m_gridMaps[0], resp_ros_.map);
00086     if(debug_) printf("msg:         %i x %i @ %4.3fm/p, %4.3f, %4.3f, %4.3f, %4.3f\n",
00087                       resp_ros_.map.info.width, resp_ros_.map.info.height, resp_ros_.map.info.resolution,
00088                       resp_ros_.map.info.origin.position.x, resp_ros_.map.info.origin.position.y,
00089                       resp_ros_.map.info.width*resp_ros_.map.info.resolution+resp_ros_.map.info.origin.position.x,
00090                       resp_ros_.map.info.height*resp_ros_.map.info.resolution+resp_ros_.map.info.origin.position.y);
00091 }
00092 
00093 bool MapServer::mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res )
00094 {
00095   ROS_INFO("mapCallback: service requested!\n");
00096   res = resp_ros_;
00097   return true;
00098 }
00099 
00100 void MapServer::publishMap () {
00101     resp_ros_.map.header.stamp = ros::Time::now();
00102     resp_ros_.map.header.seq = loop_count_;
00103     if(pub_map_ros_.getNumSubscribers() > 0){
00104         pub_map_ros_.publish(resp_ros_.map );
00105     }
00106     if(pub_metadata_.getNumSubscribers() > 0){
00107         pub_metadata_.publish( resp_ros_.map.info );
00108     }
00109 }
00110 
00111 void MapServer::loop() {
00112     if(frequency_ > 0){
00113         ros::Rate rate(frequency_);
00114         for(loop_count_ = 0; ros::ok(); loop_count_++) {
00115           publishMap ();
00116           ros::spinOnce();
00117           rate.sleep();
00118         }
00119     } else {
00120         publishMap ();
00121         ros::spin();
00122     }
00123 }
00124 
00125 int main(int argc, char **argv)
00126 {
00127     // Initialize ROS
00128     ros::init(argc, argv, "mrpt_map_server");
00129     ros::NodeHandle node;
00130     MapServer my_node(node);
00131     my_node.init();
00132     my_node.loop();
00133     return 0;
00134 }


mrpt_map
Author(s):
autogenerated on Mon Aug 11 2014 11:23:32