00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <map_server_node.h>
00030
00031 #include <mrpt_bridge/map.h>
00032
00033 #include <mrpt/system/filesystem.h>
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
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 }