
/*
 * 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 */

#ifndef __MAP_SERVER_H_INCLUDED_
#define __MAP_SERVER_H_INCLUDED_

#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <boost/filesystem.hpp>

#include "ros/ros.h"
#include "ros/console.h"
#include "map_server/image_loader.h"
#include "nav_msgs/MapMetaData.h"
#include "nav_msgs/LoadMap.h"
#include "yaml-cpp/yaml.h"

#ifdef HAVE_YAMLCPP_GT_0_5_0
// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template <typename T>
void operator>>(const YAML::Node& node, T& i)
{
  i = node.as<T>();
}
#endif
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

namespace map_server
{

  class MapServer
  {
  public:
    /**
     * @brief Trivial constructor
    */
    MapServer(const std::string& fname, double res);
    MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res);

    /**
     * @brief Load a map given a path to a yaml file
     * @param path_to_yaml The path to file yaml
     * @return True/False If load map succesed or not done
     */
    bool loadMapFromYaml(std::string path_to_yaml);

  private:
    ros::NodeHandle nh_;
    ros::Publisher map_pub_;
    ros::Publisher map_convert_;
    ros::Publisher metadata_pub_;
    ros::ServiceServer get_map_service_;
    ros::ServiceServer change_map_srv_;
    bool deprecated_;
    std::string frame_id_;
    int type_;

    void initialize(ros::NodeHandle nh, const std::string& fname, double res); 

    /**
     * @brief Callback invoked when someone requests our service
     */
    bool mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res);

    /**
     * @brief Callback invoked when someone requests to change the map
     */
    bool changeMapCallback(nav_msgs::LoadMap::Request& request, nav_msgs::LoadMap::Response& response);

    /**
     * @brief Load a map given all the values needed to understand it
     */
    bool loadMapFromValues(std::string map_file_name, double resolution,
      int negate, double occ_th, double free_th, double origin[3], MapMode mode);


    /**
     * @brief Load a map using the deprecated method
     */
    bool loadMapFromParams(std::string map_file_name, double resolution);

    /** 
     * @brief The map data is cached here, to be sent out to service callers
     */
    nav_msgs::MapMetaData meta_data_message_;
    nav_msgs::GetMap::Response map_resp_;

  }; // class MapServer

} // namespace map_sever 

#endif // __MAP_SERVER_H_INCLUDED_