git commit -m "first commit for v2"

2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
--- a/Localizations/Libraries/Ros/map_server/CHANGELOG.rst
+++ b/Localizations/Libraries/Ros/map_server/CHANGELOG.rst
@@ -0,0 +1,212 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package map_server
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.17.3 (2023-01-10)
+-------------------
+* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
+  * do not specify obsolete c++11 standard
+  this breaks with current versions of log4cxx.
+  * update pluginlib include paths
+  the non-hpp headers have been deprecated since kinetic
+  * use lambdas in favor of boost::bind
+  Using boost's _1 as a global system is deprecated since C++11.
+  The ROS packages in Debian removed the implicit support for the global symbols,
+  so this code fails to compile there without the patch.
+* Contributors: Michael Görner
+
+1.17.2 (2022-06-20)
+-------------------
+* Change_map service to map_server [Rebase/Noetic] (`#1029 <https://github.com/ros-planning/navigation/issues/1029>`_)
+  * Refactored map loading from constructor to three methods
+  * Added change_map service using LoadMap.srv
+* map_server: Initialise a NodeHandle in main. (`#1122 <https://github.com/ros-planning/navigation/issues/1122>`_)
+* Add debug output regarding waiting for time (`#1078 <https://github.com/ros-planning/navigation/issues/1078>`_)
+  Added debug messages suggested in https://github.com/ros-planning/navigation/issues/1074#issuecomment-751557177. Makes it easier to discover if use_sim_time is true but no clock server is running
+* crop_map: Fix extra pixel origin shift up every cropping (`#1064 <https://github.com/ros-planning/navigation/issues/1064>`_) (`#1067 <https://github.com/ros-planning/navigation/issues/1067>`_)
+  Co-authored-by: Pavlo Kolomiiets <pavlo@blindnology.com>
+* (map_server) add rtest dependency to tests (`#1061 <https://github.com/ros-planning/navigation/issues/1061>`_)
+* [noetic] MapServer variable cleanup: Precursor for `#1029 <https://github.com/ros-planning/navigation/issues/1029>`_ (`#1043 <https://github.com/ros-planning/navigation/issues/1043>`_)
+* Contributors: Christian Fritz, David V. Lu!!, Matthijs van der Burgh, Nikos Koukis, Pavlo Kolomiiets
+
+1.17.1 (2020-08-27)
+-------------------
+* Initial map_server map_mode tests (`#1006 <https://github.com/ros-planning/navigation/issues/1006>`_)
+* [noetic] Adding CMake way to find yaml-cpp (`#998 <https://github.com/ros-planning/navigation/issues/998>`_)
+* Contributors: David V. Lu!!, Sean Yen
+
+1.17.0 (2020-04-02)
+-------------------
+* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
+  Noetic Migration
+* increase required cmake version
+* Contributors: Michael Ferguson
+
+1.16.6 (2020-03-18)
+-------------------
+
+1.16.5 (2020-03-15)
+-------------------
+* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
+* Contributors: Sean Yen
+
+1.16.4 (2020-03-04)
+-------------------
+
+1.16.3 (2019-11-15)
+-------------------
+* Merge branch 'melodic-devel' into layer_clear_area-melodic
+* Merge pull request `#850 <https://github.com/ros-planning/navigation/issues/850>`_ from seanyen/map_server_windows_fix
+  [Windows][melodic] map_server Windows build bring up
+* map_server Windows build bring up
+  * Fix install location for Windows build. (On Windows build, shared library uses RUNTIME location, but not LIBRARY)
+  * Use boost::filesystem::path to handle path logic and remove the libgen.h dependency for better cross platform.
+  * Fix gtest hard-coded and add YAML library dir in CMakeList.txt.
+* Contributors: Michael Ferguson, Sean Yen, Steven Macenski
+
+1.16.2 (2018-07-31)
+-------------------
+
+1.16.1 (2018-07-28)
+-------------------
+
+1.16.0 (2018-07-25)
+-------------------
+* Merge pull request `#735 <https://github.com/ros-planning/navigation/issues/735>`_ from ros-planning/melodic_708
+  Allow specification of free/occupied thresholds for map_saver (`#708 <https://github.com/ros-planning/navigation/issues/708>`_)
+* Allow specification of free/occupied thresholds for map_saver (`#708 <https://github.com/ros-planning/navigation/issues/708>`_)
+  * add occupied threshold command line parameter to map_saver (--occ)
+  * add free threshold command line parameter to map_saver (--free)
+* Merge pull request `#704 <https://github.com/ros-planning/navigation/issues/704>`_ from DLu/fix573_lunar
+  Map server wait for a valid time fix [lunar]
+* Map server wait for a valid time, fix `#573 <https://github.com/ros-planning/navigation/issues/573>`_ (`#700 <https://github.com/ros-planning/navigation/issues/700>`_)
+  When launching the map_server with Gazebo, the current time is picked
+  before the simulation is started and then has a value of 0.
+  Later when querying the stamp of the map, a value of has a special
+  signification on tf transform for example.
+* Contributors: Michael Ferguson, Romain Reignier, ipa-fez
+
+1.15.2 (2018-03-22)
+-------------------
+* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
+  update maintainer email (lunar)
+* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
+  Add myself as a maintainer.
+* Rebase PRs from Indigo/Kinetic (`#637 <https://github.com/ros-planning/navigation/issues/637>`_)
+  * Respect planner_frequency intended behavior (`#622 <https://github.com/ros-planning/navigation/issues/622>`_)
+  * Only do a getRobotPose when no start pose is given (`#628 <https://github.com/ros-planning/navigation/issues/628>`_)
+  Omit the unnecessary call to getRobotPose when the start pose was
+  already given, so that move_base can also generate a path in
+  situations where getRobotPose would fail.
+  This is actually to work around an issue of getRobotPose randomly
+  failing.
+  * Update gradient_path.cpp (`#576 <https://github.com/ros-planning/navigation/issues/576>`_)
+  * Update gradient_path.cpp
+  * Update navfn.cpp
+  * update to use non deprecated pluginlib macro (`#630 <https://github.com/ros-planning/navigation/issues/630>`_)
+  * update to use non deprecated pluginlib macro
+  * multiline version as well
+  * Print SDL error on IMG_Load failure in server_map (`#631 <https://github.com/ros-planning/navigation/issues/631>`_)
+* Use occupancy values when saving a map (`#613 <https://github.com/ros-planning/navigation/issues/613>`_)
+* Closes `#625 <https://github.com/ros-planning/navigation/issues/625>`_ (`#627 <https://github.com/ros-planning/navigation/issues/627>`_)
+* Contributors: Aaron Hoy, David V. Lu!!, Hunter Allen, Michael Ferguson
+
+1.15.1 (2017-08-14)
+-------------------
+* remove offending library export (fixes `#612 <https://github.com/ros-planning/navigation/issues/612>`_)
+* Contributors: Michael Ferguson
+
+1.15.0 (2017-08-07)
+-------------------
+* Fix compiler warning for GCC 8.
+* convert packages to format2
+* Merge pull request `#596 <https://github.com/ros-planning/navigation/issues/596>`_ from ros-planning/lunar_548
+* refactor to not use tf version 1 (`#561 <https://github.com/ros-planning/navigation/issues/561>`_)
+* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
+* Merge pull request `#560 <https://github.com/ros-planning/navigation/issues/560>`_ from wjwwood/map_server_fixup_cmake
+* update to support Python 2 and 3 (`#559 <https://github.com/ros-planning/navigation/issues/559>`_)
+* remove duplicate and unreferenced file (`#558 <https://github.com/ros-planning/navigation/issues/558>`_)
+* remove trailing whitespace from map_server package (`#557 <https://github.com/ros-planning/navigation/issues/557>`_)
+* fix cmake use of yaml-cpp and sdl / sdl-image
+* Fix CMake warnings
+* Contributors: Hunter L. Allen, Martin Günther, Michael Ferguson, Mikael Arguedas, Vincent Rabaud, William Woodall
+
+1.14.0 (2016-05-20)
+-------------------
+* Corrections to alpha channel detection and usage.
+  Changing to actually detect whether the image has an alpha channel instead of
+  inferring based on the number of channels.
+  Also reverting to legacy behavior of trinary mode overriding alpha removal.
+  This will cause the alpha channel to be averaged in with the others in trinary
+  mode, which is the current behavior before this PR.
+* Removing some trailing whitespace.
+* Use enum to control map interpretation
+* Contributors: Aaron Hoy, David Lu
+
+1.13.1 (2015-10-29)
+-------------------
+
+1.13.0 (2015-03-17)
+-------------------
+* rename image_loader library, fixes `#208 <https://github.com/ros-planning/navigation/issues/208>`_
+* Contributors: Michael Ferguson
+
+1.12.0 (2015-02-04)
+-------------------
+* update maintainer email
+* Contributors: Michael Ferguson
+
+1.11.15 (2015-02-03)
+--------------------
+
+1.11.14 (2014-12-05)
+--------------------
+* prevent inf loop
+* Contributors: Jeremie Deray
+
+1.11.13 (2014-10-02)
+--------------------
+
+1.11.12 (2014-10-01)
+--------------------
+* map_server: [style] alphabetize dependencies
+* map_server: remove vestigial export line
+  the removed line does not do anything in catkin
+* Contributors: William Woodall
+
+1.11.11 (2014-07-23)
+--------------------
+
+1.11.10 (2014-06-25)
+--------------------
+
+1.11.9 (2014-06-10)
+-------------------
+
+1.11.8 (2014-05-21)
+-------------------
+* fix build, was broken by `#175 <https://github.com/ros-planning/navigation/issues/175>`_
+* Contributors: Michael Ferguson
+
+1.11.7 (2014-05-21)
+-------------------
+* make rostest in CMakeLists optional
+* Contributors: Lukas Bulwahn
+
+1.11.5 (2014-01-30)
+-------------------
+* install crop map
+* removing .py from executable script
+* Map Server can serve maps with non-lethal values
+* Added support for YAML-CPP 0.5+.
+  The new yaml-cpp API removes the "node >> outputvar;" operator, and
+  it has a new way of loading documents. There's no version hint in the
+  library's headers, so I'm getting the version number from pkg-config.
+* check for CATKIN_ENABLE_TESTING
+* Change maintainer from Hersh to Lu
+
+1.11.4 (2013-09-27)
+-------------------
+* prefix utest target to not collide with other targets
+* Package URL Updates
+* unique target names to avoid conflicts (e.g. with map-store)
--- a/Localizations/Libraries/Ros/map_server/CMakeLists.txt
+++ b/Localizations/Libraries/Ros/map_server/CMakeLists.txt
@@ -0,0 +1,153 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(map_server)
+
+find_package(catkin REQUIRED
+        COMPONENTS
+            roscpp
+            nav_msgs
+            tf2
+        )
+
+find_package(Bullet REQUIRED)
+find_package(SDL REQUIRED)
+find_package(SDL_image REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAMLCPP yaml-cpp QUIET)
+if(NOT YAMLCPP_FOUND)
+  find_package(yaml-cpp 0.6 REQUIRED)
+  set(YAMLCPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR})
+  set(YAMLCPP_LIBRARIES ${YAML_CPP_LIBRARIES})
+  add_definitions(-DHAVE_YAMLCPP_GT_0_5_0)
+else()
+  if(YAMLCPP_VERSION VERSION_GREATER "0.5.0")
+    add_definitions(-DHAVE_YAMLCPP_GT_0_5_0)
+  endif()
+  link_directories(${YAMLCPP_LIBRARY_DIRS})
+endif()
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES map_server_lib map_server_image_loader map_generator_lib
+    CATKIN_DEPENDS roscpp nav_msgs tf2
+)
+
+include_directories(
+    include
+    ${BULLET_INCLUDE_DIRS}
+    ${catkin_INCLUDE_DIRS}
+    ${SDL_INCLUDE_DIR}
+    ${SDL_IMAGE_INCLUDE_DIRS}
+    ${YAMLCPP_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+)
+
+add_library(map_server_image_loader 
+            src/image_loader.cpp
+            src/image_16bit_loader.cpp
+            src/image_32bit_loader.cpp
+          )
+add_dependencies(map_server_image_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(map_server_image_loader
+    ${BULLET_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${SDL_LIBRARY}
+    ${SDL_IMAGE_LIBRARIES}
+    ${YAMLCPP_LIBRARIES}
+)
+
+add_library(map_server_lib src/map_server.cpp)
+add_dependencies(map_server_lib map_server_image_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(map_server_lib
+    map_server_image_loader
+    ${BULLET_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${SDL_LIBRARY}
+    ${SDL_IMAGE_LIBRARIES}
+    ${YAMLCPP_LIBRARIES}
+)
+
+add_library (map_generator_lib src/map_generator.cpp)
+add_dependencies(map_generator_lib  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(map_generator_lib
+    ${BULLET_LIBRARIES}
+    ${catkin_LIBRARIES}
+    ${SDL_LIBRARY}
+    ${SDL_IMAGE_LIBRARIES}
+    ${YAMLCPP_LIBRARIES}
+)
+
+add_executable(map_server src/main.cpp)
+add_dependencies(map_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(map_server
+    map_server_lib
+    ${YAMLCPP_LIBRARIES}
+    ${catkin_LIBRARIES}
+)
+
+add_executable(map_server-map_saver src/map_saver.cpp)
+add_dependencies(map_server-map_saver map_generator_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+set_target_properties(map_server-map_saver PROPERTIES OUTPUT_NAME map_saver)
+target_link_libraries(map_server-map_saver
+    map_generator_lib
+    ${catkin_LIBRARIES}
+)
+
+# copy test data to same place as tests are run
+function(copy_test_data)
+    cmake_parse_arguments(PROJECT "" "" "FILES" ${ARGN})
+    foreach(datafile ${PROJECT_FILES})
+        file(COPY ${datafile} DESTINATION ${PROJECT_BINARY_DIR}/test)
+    endforeach()
+endfunction()
+
+## Tests
+if(CATKIN_ENABLE_TESTING)
+  copy_test_data( FILES
+      test/testmap.bmp
+      test/testmap.png
+      test/spectrum.png )
+  catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp test/test_constants.cpp)
+  target_link_libraries(${PROJECT_NAME}_utest
+    map_server_lib
+    ${SDL_LIBRARY}
+    ${SDL_IMAGE_LIBRARIES}
+  )
+
+  find_package(roslib REQUIRED)
+  include_directories(${roslib_INCLUDE_DIRS})
+  add_executable(rtest test/rtest.cpp test/test_constants.cpp)
+  add_dependencies(rtest ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(tests rtest)
+  target_link_libraries( rtest
+      ${GTEST_LIBRARIES}
+      ${catkin_LIBRARIES}
+      ${roslib_LIBRARIES}
+  )
+
+  # This has to be done after we've already built targets, or catkin variables get borked
+  find_package(rostest REQUIRED)
+  add_rostest(test/rtest.xml)
+endif()
+
+## Install executables and/or libraries
+install(TARGETS map_server-map_saver map_server
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(TARGETS map_server_image_loader map_server_lib map_generator_lib
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)
+
+## Install excutable python script
+install(
+    PROGRAMS
+      scripts/crop_map
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
--- a/Localizations/Libraries/Ros/map_server/include/map_server/image_loader.h
+++ b/Localizations/Libraries/Ros/map_server/include/map_server/image_loader.h
@@ -0,0 +1,115 @@
+/*
+ * 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.
+ */
+#ifndef MAP_SERVER_MAP_SERVER_H
+#define MAP_SERVER_MAP_SERVER_H
+
+/*
+ * Author: Brian Gerkey
+ */
+
+#include "nav_msgs/GetMap.h"
+
+/** Map mode
+ *  Default: TRINARY -
+ *      value >= occ_th - Occupied (100)
+ *      value <= free_th - Free (0)
+ *      otherwise - Unknown
+ *  SCALE -
+ *      alpha < 1.0 - Unknown
+ *      value >= occ_th - Occupied (100)
+ *      value <= free_th - Free (0)
+ *      otherwise - f( (free_th, occ_th) ) = (0, 100)
+ *          (linearly map in between values to (0,100)
+ *  RAW -
+ *      value = value
+ */
+enum MapMode {TRINARY, SCALE, RAW};
+
+namespace map_server
+{
+
+/** Read the image from file and fill out the resp object, for later
+ * use when our services are requested.
+ *
+ * @param resp The map wil be written into here
+ * @param fname The image file to read from
+ * @param res The resolution of the map (gets stored in resp)
+ * @param negate If true, then whiter pixels are occupied, and blacker
+ *               pixels are free
+ * @param occ_th Threshold above which pixels are occupied
+ * @param free_th Threshold below which pixels are free
+ * @param origin Triple specifying 2-D pose of lower-left corner of image
+ * @param mode Map mode
+ * @throws std::runtime_error If the image file can't be loaded
+ * */
+void loadMapFromFile(nav_msgs::GetMap::Response* resp,
+                     const char* fname, double res, bool negate,
+                     double occ_th, double free_th, double* origin,
+                     MapMode mode=TRINARY);
+
+/** Read the image from file and fill out the resp object, for later
+ * use when our services are requested.
+ *
+ * @param resp The map wil be written into here
+ * @param fname The image file to read from
+ * @param res The resolution of the map (gets stored in resp)
+ * @param negate If true, then whiter pixels are occupied, and blacker
+ *               pixels are free
+ * @param occ_th Threshold above which pixels are occupied
+ * @param free_th Threshold below which pixels are free
+ * @param origin Triple specifying 2-D pose of lower-left corner of image
+ * @param mode Map mode
+ * @throws std::runtime_error If the image file can't be loaded
+ * */
+void loadMapFromFile16Bit(nav_msgs::GetMap::Response* resp,
+                     const char* fname, double res, bool negate,
+                     double occ_th, double free_th, double* origin,
+                     MapMode mode=TRINARY);
+
+/** Read the image from file and fill out the resp object, for later
+ * use when our services are requested.
+ *
+ * @param resp The map wil be written into here
+ * @param fname The image file to read from
+ * @param res The resolution of the map (gets stored in resp)
+ * @param negate If true, then whiter pixels are occupied, and blacker
+ *               pixels are free
+ * @param occ_th Threshold above which pixels are occupied
+ * @param free_th Threshold below which pixels are free
+ * @param origin Triple specifying 2-D pose of lower-left corner of image
+ * @param mode Map mode
+ * @throws std::runtime_error If the image file can't be loaded
+ * */
+void loadMapFromFile32Bit(nav_msgs::GetMap::Response* resp,
+                     const char* fname, double res, bool negate,
+                     double occ_th, double free_th, double* origin,
+                     MapMode mode=TRINARY);
+}
+
+#endif
--- a/Localizations/Libraries/Ros/map_server/include/map_server/map_generator.h
+++ b/Localizations/Libraries/Ros/map_server/include/map_server/map_generator.h
@@ -0,0 +1,43 @@
+#ifndef MAP_GENERATOR_H_INCLUDED_
+#define MAP_GENERATOR_H_INCLUDED_
+
+#include <cstdio>
+#include "ros/ros.h"
+#include "ros/console.h"
+#include "nav_msgs/GetMap.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "geometry_msgs/Quaternion.h"
+
+
+namespace map_server
+{
+  /**
+   * @brief Map generation node.
+   */
+  class MapGenerator
+  {
+  public:
+    MapGenerator(ros::NodeHandle nh, const std::string& mapname, const std::string map_topic, 
+                  int threshold_occupied = 65, int threshold_free = 25);
+                  
+    MapGenerator(const std::string& mapname, const std::string map_topic, 
+                  int threshold_occupied = 65, int threshold_free = 25);
+    virtual ~MapGenerator() {}
+    virtual bool  waitingGenerator(double tolerance = 2.0);
+    bool isMapSaved() {return saved_map_;}
+
+  private:
+
+    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map);
+
+    std::string mapname_;
+    ros::Subscriber map_sub_;
+    bool saved_map_;
+    int threshold_occupied_;
+    int threshold_free_;
+  };
+
+
+} // namespace map_server
+
+#endif // MAP_GENERATOR_H_INCLUDED_
--- a/Localizations/Libraries/Ros/map_server/include/map_server/map_server.h
+++ b/Localizations/Libraries/Ros/map_server/include/map_server/map_server.h
@@ -0,0 +1,123 @@
+
+/*
+ * 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_
--- a/Localizations/Libraries/Ros/map_server/launch/test.launch
+++ b/Localizations/Libraries/Ros/map_server/launch/test.launch
@@ -0,0 +1,4 @@
+<launch>
+  <include file="$(find map_server)/test/rtest.xml"/>
+  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find sbpl_lattice_planner)/rviz/sbpl.rviz" /> -->
+</launch>
--- a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_0.png
+++ b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_0.png
--- a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_180.png
+++ b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_180.png
--- a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_270.png
+++ b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_270.png
--- a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_90.png
+++ b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_90.png
--- a/Localizations/Libraries/Ros/map_server/maps/maze.png
+++ b/Localizations/Libraries/Ros/map_server/maps/maze.png
--- a/Localizations/Libraries/Ros/map_server/maps/maze.yaml
+++ b/Localizations/Libraries/Ros/map_server/maps/maze.yaml
@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
--- a/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.png
+++ b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.png
--- a/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.yaml
+++ b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.yaml
@@ -0,0 +1,6 @@
+image: maze_virtual_walls.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
--- a/Localizations/Libraries/Ros/map_server/package.xml
+++ b/Localizations/Libraries/Ros/map_server/package.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+    <name>map_server</name>
+    <version>1.17.3</version>
+    <description>
+
+        map_server provides the <tt>map_server</tt> ROS <a href="http://www.ros.org/wiki/Nodes">Node</a>, which offers map data as a ROS <a href="http://www.ros.org/wiki/Services">Service</a>. It also provides the <tt>map_saver</tt> command-line utility, which allows dynamically generated maps to be saved to file.
+
+    </description>
+    <author>Brian Gerkey, Tony Pratkanis</author>
+    <author>contradict@gmail.com</author>
+    <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+    <maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
+    <maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
+    <url>http://wiki.ros.org/map_server</url>
+    <license>BSD</license>
+
+    <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+    <depend>bullet</depend>
+    <depend>nav_msgs</depend>
+    <depend>roscpp</depend>
+    <depend>sdl</depend>
+    <depend>sdl-image</depend>
+    <depend>tf2</depend>
+    <depend>yaml-cpp</depend>
+
+    <test_depend>roslib</test_depend>
+    <test_depend>rospy</test_depend>
+    <test_depend>rostest</test_depend>
+    <test_depend>rosunit</test_depend>
+</package>
--- a/Localizations/Libraries/Ros/map_server/scripts/crop_map
+++ b/Localizations/Libraries/Ros/map_server/scripts/crop_map
@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# 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 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.
+#
+
+from __future__ import print_function
+
+import sys
+import yaml
+from PIL import Image
+import math
+
+def find_bounds(map_image):
+    x_min = map_image.size[0]
+    x_end = 0
+    y_min = map_image.size[1]
+    y_end = 0
+    pix = map_image.load()
+    for x in range(map_image.size[0]):
+        for y in range(map_image.size[1]):
+            val = pix[x, y]
+            if val != 205:  # not unknown
+                x_min = min(x, x_min)
+                x_end = max(x, x_end)
+                y_min = min(y, y_min)
+                y_end = max(y, y_end)
+    return x_min, x_end, y_min, y_end
+
+def computed_cropped_origin(map_image, bounds, resolution, origin):
+    """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """
+    ox = origin[0]
+    oy = origin[1]
+    oth = origin[2]
+
+    # First figure out the delta we have to translate from the lower left corner (which is the origin)
+    # in the image system
+    dx = bounds[0] * resolution
+    dy = (map_image.size[1] - bounds[3] - 1) * resolution
+
+    # Next rotate this by the theta and add to the old origin
+
+    new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth)
+    new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth)
+
+    return [new_ox, new_oy, oth]
+
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print("Usage: %s map.yaml [cropped.yaml]" % sys.argv[0], file=sys.stderr)
+        sys.exit(1)
+
+    with open(sys.argv[1]) as f:
+        map_data = yaml.safe_load(f)
+
+    if len(sys.argv) > 2:
+        crop_name = sys.argv[2]
+        if crop_name.endswith(".yaml"):
+            crop_name = crop_name[:-5]
+        crop_yaml = crop_name + ".yaml"
+        crop_image = crop_name + ".pgm"
+    else:
+        crop_yaml = "cropped.yaml"
+        crop_image = "cropped.pgm"
+
+    map_image_file = map_data["image"]
+    resolution = map_data["resolution"]
+    origin = map_data["origin"]
+    map_image = Image.open(map_image_file)
+
+    bounds = find_bounds(map_image)
+
+    # left, upper, right, lower
+    cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1))
+
+    cropped_image.save(crop_image)
+    map_data["image"] = crop_image
+    map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin)
+    with open(crop_yaml, "w") as f:
+        yaml.dump(map_data, f)
+
--- a/Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp
@@ -0,0 +1,133 @@
+/*
+ * 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.
+ */
+
+/*
+ * This file contains helper functions for loading images as maps.
+ *
+ * Author: Brian Gerkey
+ */
+
+#include <cstring>
+#include <stdexcept>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+// We use SDL_image to load the image from disk
+#include <SDL/SDL_image.h>
+
+// Use Bullet's Quaternion object to create one from Euler angles
+#include <LinearMath/btQuaternion.h>
+
+#include "map_server/image_loader.h"
+#include <ros/ros.h>
+
+// compute linear index for given map coords
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+namespace map_server
+{
+
+void loadMapFromFile16Bit(nav_msgs::GetMap::Response* resp,
+                const char* fname, double res, bool negate,
+                double occ_th, double free_th, double* origin,
+                MapMode mode)
+{
+  SDL_Surface* img;
+
+  unsigned char* pixels;
+  unsigned char* p;
+  int rowstride, n_channels, avg_channels;
+  unsigned int i,j;
+  int k;
+  int alpha;
+  uint16_t color_avg;
+
+  // Load the image using SDL.  If we get NULL back, the image load failed.
+  if(!(img = IMG_Load(fname)))
+  {
+    std::string errmsg = std::string("failed to open image file \"") +
+            std::string(fname) + std::string("\": ") + IMG_GetError();
+    throw std::runtime_error(errmsg);
+  }
+
+  // Copy the image data into the map structure
+  resp->map.info.width = img->w*2;
+  resp->map.info.height = img->h;
+  resp->map.info.resolution = res;
+  resp->map.info.origin.position.x = *(origin);
+  resp->map.info.origin.position.y = *(origin+1);
+  resp->map.info.origin.position.z = 0.0;
+  btQuaternion q;
+  // setEulerZYX(yaw, pitch, roll)
+  q.setEulerZYX(*(origin+2), 0, 0);
+  resp->map.info.origin.orientation.x = q.x();
+  resp->map.info.origin.orientation.y = q.y();
+  resp->map.info.origin.orientation.z = q.z();
+  resp->map.info.origin.orientation.w = q.w();
+
+  // Allocate space to hold the data
+  resp->map.data.resize(resp->map.info.width * resp->map.info.height);
+
+  // Get values that we'll need to iterate through the pixels
+  rowstride = img->pitch;
+  n_channels = img->format->BytesPerPixel;
+
+  // NOTE: Trinary mode still overrides here to preserve existing behavior.
+  // Alpha will be averaged in with color channels when using trinary mode.
+  if (mode==TRINARY || !img->format->Amask)
+    avg_channels = n_channels;
+  else
+    avg_channels = n_channels - 1;
+  // Copy pixel data into the map structure
+  pixels = (unsigned char*)(img->pixels);
+  for(j = 0; j < resp->map.info.height; j++)
+  {
+    for (i = 0; i < resp->map.info.width; i+=2)
+    {
+      // Compute mean of RGB for this pixel
+      p = pixels + j*rowstride + i/2*n_channels;
+      color_avg = (*(p) << 16) + (*(p+1) << 8) + (*(p+2));
+      if (n_channels == 1)
+          alpha = 1;
+      else
+          alpha = *(p+n_channels-1);
+
+      if(negate)
+        color_avg = 65535 - color_avg;
+
+			resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = (unsigned char)(color_avg & 0x00FF);
+			resp->map.data[MAP_IDX(resp->map.info.width,i+1,resp->map.info.height - j - 1)] = (unsigned char)((color_avg & 0xFF00) >> 8);
+    }
+  }
+
+  SDL_FreeSurface(img);
+}
+
+}
--- a/Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp
@@ -0,0 +1,134 @@
+/*
+ * 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.
+ */
+
+/*
+ * This file contains helper functions for loading images as maps.
+ *
+ * Author: Brian Gerkey
+ */
+
+#include <cstring>
+#include <stdexcept>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+// We use SDL_image to load the image from disk
+#include <SDL/SDL_image.h>
+
+// Use Bullet's Quaternion object to create one from Euler angles
+#include <LinearMath/btQuaternion.h>
+
+#include "map_server/image_loader.h"
+#include <ros/ros.h>
+
+// compute linear index for given map coords
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+namespace map_server
+{
+  void loadMapFromFile32Bit(nav_msgs::GetMap::Response *resp,
+                            const char *fname, double res, bool negate,
+                            double occ_th, double free_th, double *origin,
+                            MapMode mode)
+  {
+    SDL_Surface *img;
+
+    unsigned char *pixels;
+    unsigned char *p;
+    int rowstride, n_channels, avg_channels;
+    unsigned int i, j;
+    int k;
+    int alpha;
+    uint32_t color_avg;
+
+    // Load the image using SDL.  If we get NULL back, the image load failed.
+    if (!(img = IMG_Load(fname)))
+    {
+      std::string errmsg = std::string("failed to open image file \"") +
+                           std::string(fname) + std::string("\": ") + IMG_GetError();
+      throw std::runtime_error(errmsg);
+    }
+
+    // Copy the image data into the map structure
+    resp->map.info.width = img->w * 2;
+    resp->map.info.height = img->h * 2;
+    resp->map.info.resolution = res;
+    resp->map.info.origin.position.x = *(origin);
+    resp->map.info.origin.position.y = *(origin + 1);
+    resp->map.info.origin.position.z = 0.0;
+    btQuaternion q;
+    // setEulerZYX(yaw, pitch, roll)
+    q.setEulerZYX(*(origin + 2), 0, 0);
+    resp->map.info.origin.orientation.x = q.x();
+    resp->map.info.origin.orientation.y = q.y();
+    resp->map.info.origin.orientation.z = q.z();
+    resp->map.info.origin.orientation.w = q.w();
+
+    // Allocate space to hold the data
+    resp->map.data.resize(resp->map.info.width * resp->map.info.height);
+
+    // Get values that we'll need to iterate through the pixels
+    rowstride = img->pitch;
+    n_channels = img->format->BytesPerPixel;
+
+    // NOTE: Trinary mode still overrides here to preserve existing behavior.
+    // Alpha will be averaged in with color channels when using trinary mode.
+    if (mode == TRINARY || !img->format->Amask)
+      avg_channels = n_channels;
+    else
+      avg_channels = n_channels - 1;
+    // Copy pixel data into the map structure
+    pixels = (unsigned char *)(img->pixels);
+    for (j = 0; j < resp->map.info.height; j += 2)
+    {
+      for (i = 0; i < resp->map.info.width; i += 2)
+      {
+        // Compute mean of RGB for this pixel
+        p = pixels + j / 2 * rowstride + i / 2 * n_channels;
+        color_avg = (*(p) << 16) + (*(p + 1) << 8) + (*(p + 2));
+        if (n_channels == 1)
+          alpha = 1;
+        else
+          alpha = *(p + n_channels - 1);
+
+        if (negate)
+          color_avg = 16777215 - color_avg;
+
+        resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 1)] = (color_avg >> 16u) & 0xFF;
+        resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 1)] = (color_avg >> 24u) & 0xFF;
+        resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 2)] = (color_avg >> 0u) & 0xFF;
+        resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 2)] = (color_avg >> 8u) & 0xFF;
+      }
+    }
+
+    SDL_FreeSurface(img);
+  }
+
+}
--- a/Localizations/Libraries/Ros/map_server/src/image_loader.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/image_loader.cpp
@@ -0,0 +1,167 @@
+/*
+ * 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.
+ */
+
+/*
+ * This file contains helper functions for loading images as maps.
+ *
+ * Author: Brian Gerkey
+ */
+
+#include <cstring>
+#include <stdexcept>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+// We use SDL_image to load the image from disk
+#include <SDL/SDL_image.h>
+
+// Use Bullet's Quaternion object to create one from Euler angles
+#include <LinearMath/btQuaternion.h>
+
+#include "map_server/image_loader.h"
+#include <ros/ros.h>
+
+// compute linear index for given map coords
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+namespace map_server
+{
+
+void
+loadMapFromFile(nav_msgs::GetMap::Response* resp,
+                const char* fname, double res, bool negate,
+                double occ_th, double free_th, double* origin,
+                MapMode mode)
+{
+  SDL_Surface* img;
+
+  unsigned char* pixels;
+  unsigned char* p;
+  unsigned char value;
+  int rowstride, n_channels, avg_channels;
+  unsigned int i,j;
+  int k;
+  double occ;
+  int alpha;
+  int color_sum;
+  double color_avg;
+
+  // Load the image using SDL.  If we get NULL back, the image load failed.
+  if(!(img = IMG_Load(fname)))
+  {
+    std::string errmsg = std::string("failed to open image file \"") +
+            std::string(fname) + std::string("\": ") + IMG_GetError();
+    throw std::runtime_error(errmsg);
+  }
+
+  // Copy the image data into the map structure
+  resp->map.info.width = img->w;
+  resp->map.info.height = img->h;
+  resp->map.info.resolution = res;
+  resp->map.info.origin.position.x = *(origin);
+  resp->map.info.origin.position.y = *(origin+1);
+  resp->map.info.origin.position.z = 0.0;
+  btQuaternion q;
+  // setEulerZYX(yaw, pitch, roll)
+  q.setEulerZYX(*(origin+2), 0, 0);
+  resp->map.info.origin.orientation.x = q.x();
+  resp->map.info.origin.orientation.y = q.y();
+  resp->map.info.origin.orientation.z = q.z();
+  resp->map.info.origin.orientation.w = q.w();
+
+  // Allocate space to hold the data
+  resp->map.data.resize(resp->map.info.width * resp->map.info.height);
+
+  // Get values that we'll need to iterate through the pixels
+  rowstride = img->pitch;
+  n_channels = img->format->BytesPerPixel;
+
+  // NOTE: Trinary mode still overrides here to preserve existing behavior.
+  // Alpha will be averaged in with color channels when using trinary mode.
+  if (mode==TRINARY || !img->format->Amask)
+    avg_channels = n_channels;
+  else
+    avg_channels = n_channels - 1;
+  // Copy pixel data into the map structure
+  pixels = (unsigned char*)(img->pixels);
+  for(j = 0; j < resp->map.info.height; j++)
+  {
+    for (i = 0; i < resp->map.info.width; i++)
+    {
+      // Compute mean of RGB for this pixel
+      p = pixels + j*rowstride + i*n_channels;
+      color_sum = 0;
+      int color = 0;
+      for(k=0;k<avg_channels;k++)
+      {
+        color_sum += *(p + (k));
+      }
+      color_avg = color_sum / (double)avg_channels;
+      if (n_channels == 1)
+          alpha = 1;
+      else
+          alpha = *(p+n_channels-1);
+
+      if(negate)
+        color_avg = 255 - color_avg;
+
+      if(mode==RAW){
+          value = color_avg;
+          resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
+          continue;
+      }
+
+
+      // If negate is true, we consider blacker pixels free, and whiter
+      // pixels occupied.  Otherwise, it's vice versa.
+      occ = (255 - color_avg) / 255.0;
+
+      // Apply thresholds to RGB means to determine occupancy values for
+      // map.  Note that we invert the graphics-ordering of the pixels to
+      // produce a map with cell (0,0) in the lower-left corner.
+      if(occ > occ_th)
+        value = +100;
+      else if(occ < free_th)
+        value = 0;
+      else if(mode==TRINARY || alpha < 1.0)
+        value = -1;
+      else {
+        double ratio = (occ - free_th) / (occ_th - free_th);
+        value = 1 + 98 * ratio;
+      }
+
+      resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
+    }
+  }
+
+  SDL_FreeSurface(img);
+}
+
+}
--- a/Localizations/Libraries/Ros/map_server/src/main.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/main.cpp
@@ -0,0 +1,40 @@
+
+#include "ros/ros.h"
+#include "ros/console.h"
+#include "map_server/map_server.h"
+
+#define USAGE "\nUSAGE: map_server <map.yaml>\n"                  \
+              "  map.yaml: map description file\n"                \
+              "DEPRECATED USAGE: map_server <map> <resolution>\n" \
+              "  map: image file to load\n"                       \
+              "  resolution: map resolution [meters/pixel]"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
+  // ros::init(argc, argv, "map_server");
+  ros::NodeHandle nh("~");
+  if (argc != 3 && argc != 2)
+  {
+    ROS_ERROR("%s", USAGE);
+    exit(-1);
+  }
+  if (argc != 2)
+  {
+    ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
+  }
+  std::string fname(argv[1]);
+  double res = (argc == 2) ? 0.0 : atof(argv[2]);
+  try
+  {
+    map_server::MapServer* map_sever_ptr = new map_server::MapServer(fname, res);
+    ros::spin();
+  }
+  catch (std::runtime_error& e)
+  {
+    ROS_ERROR("map_server exception: %s", e.what());
+    return -1;
+  }
+
+  return 0;
+}
--- a/Localizations/Libraries/Ros/map_server/src/map_generator.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/map_generator.cpp
@@ -0,0 +1,104 @@
+#include "map_server/map_generator.h"
+
+map_server::MapGenerator::MapGenerator(ros::NodeHandle nh, const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free)
+  : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
+{
+  ROS_INFO("Waiting for the map by topic %s", map_topic.c_str());
+  map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this);
+}
+
+map_server::MapGenerator::MapGenerator(const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free)
+  : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
+{
+  ros::NodeHandle nh;
+  ROS_INFO("Waiting for the map");
+  map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this);
+}
+
+void map_server::MapGenerator::mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
+{
+  ROS_INFO("[MapGenerator] Received a %d X %d map @ %.3f m/pix",
+    map->info.width,
+    map->info.height,
+    map->info.resolution);
+
+
+  std::string mapdatafile = mapname_ + ".pgm";
+  ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
+  FILE* out = fopen(mapdatafile.c_str(), "w");
+  if (!out)
+  {
+    ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
+    return;
+  }
+
+  fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
+    map->info.resolution, map->info.width, map->info.height);
+  for (unsigned int y = 0; y < map->info.height; y++) {
+    for (unsigned int x = 0; x < map->info.width; x++) {
+      unsigned int i = x + (map->info.height - y - 1) * map->info.width;
+      if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
+        fputc(254, out);
+      }
+      else if (map->data[i] >= threshold_occupied_) { // (occ,255]
+        fputc(000, out);
+      }
+      else { //occ [0.25,0.65]
+        fputc(205, out);
+      }
+    }
+  }
+
+  fclose(out);
+
+
+  std::string mapmetadatafile = mapname_ + ".yaml";
+  ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
+  FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
+
+
+  /*
+resolution: 0.100000
+origin: [0.000000, 0.000000, 0.000000]
+#
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
+       */
+
+  geometry_msgs::Quaternion orientation = map->info.origin.orientation;
+  tf2::Matrix3x3 mat(tf2::Quaternion(
+    orientation.x,
+    orientation.y,
+    orientation.z,
+    orientation.w
+  ));
+  double yaw, pitch, roll;
+  mat.getEulerYPR(yaw, pitch, roll);
+
+  fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
+    mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);
+
+  fclose(yaml);
+
+  ROS_INFO("Done\n");
+  saved_map_ = true;
+}
+
+bool  map_server::MapGenerator::waitingGenerator(double tolerance)
+{
+  bool result = true;
+  ros::Time start = ros::Time::now();
+  while(!this->isMapSaved() && ros::ok())
+  {
+    if((ros::Time::now() - start) > ros::Duration(tolerance))
+    {
+      result = false;
+      break;
+    }
+    ros::spinOnce();
+  }
+  if(this->isMapSaved()) result = true;
+  return result;
+}
--- a/Localizations/Libraries/Ros/map_server/src/map_saver.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/map_saver.cpp
@@ -0,0 +1,119 @@
+/*
+ * map_saver
+ * 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 <ORGANIZATION> 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.
+ */
+
+#include <ros/ros.h>
+#include "map_server/map_generator.h"
+
+#define USAGE "Usage: \n" \
+              "  map_saver -h\n"\
+              "  map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "map_saver");
+  std::string mapname = "map";
+  int threshold_occupied = 65;
+  int threshold_free = 25;
+
+  for(int i=1; i<argc; i++)
+  {
+    if(!strcmp(argv[i], "-h"))
+    {
+      puts(USAGE);
+      return 0;
+    }
+    else if(!strcmp(argv[i], "-f"))
+    {
+      if(++i < argc)
+        mapname = argv[i];
+      else
+      {
+        puts(USAGE);
+        return 1;
+      }
+    }
+    else if (!strcmp(argv[i], "--occ"))
+    {
+      if (++i < argc)
+      {
+        threshold_occupied = std::atoi(argv[i]);
+        if (threshold_occupied < 1 || threshold_occupied > 100)
+        {
+          ROS_ERROR("threshold_occupied must be between 1 and 100");
+          return 1;
+        }
+
+      }
+      else
+      {
+        puts(USAGE);
+        return 1;
+      }
+    }
+    else if (!strcmp(argv[i], "--free"))
+    {
+      if (++i < argc)
+      {
+        threshold_free = std::atoi(argv[i]);
+        if (threshold_free < 0 || threshold_free > 100)
+        {
+          ROS_ERROR("threshold_free must be between 0 and 100");
+          return 1;
+        }
+
+      }
+      else
+      {
+        puts(USAGE);
+        return 1;
+      }
+    }
+    else
+    {
+      puts(USAGE);
+      return 1;
+    }
+  }
+
+  if (threshold_occupied <= threshold_free)
+  {
+    ROS_ERROR("threshold_free must be smaller than threshold_occupied");
+    return 1;
+  }
+
+  map_server::MapGenerator mg(mapname, "map", threshold_occupied, threshold_free);
+
+  while(!mg.isMapSaved() && ros::ok())
+    ros::spinOnce();
+
+  return 0;
+}
+
+
--- a/Localizations/Libraries/Ros/map_server/src/map_server.cpp
+++ b/Localizations/Libraries/Ros/map_server/src/map_server.cpp
@@ -0,0 +1,308 @@
+
+#include "map_server/map_server.h"
+
+
+map_server::MapServer::MapServer(const std::string& fname, double res)
+{
+  ros::NodeHandle private_nh("~");
+  this->initialize(private_nh, fname, res);
+}
+
+map_server::MapServer::MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res)
+{
+  nh_ = ros::NodeHandle(name_sp);
+  this->initialize(nh, fname, res);
+}
+
+void map_server::MapServer::initialize(ros::NodeHandle nh, const std::string& fname, double res)
+{
+  std::string mapfname = "";
+  double origin[3];
+  int negate;
+  double occ_th, free_th;
+  MapMode mode = TRINARY;
+  nh.param("frame_id", frame_id_, std::string("map"));
+  nh.param("type", type_, 8);
+
+  // When called this service returns a copy of the current map
+  get_map_service_ = nh_.advertiseService("static_map", &map_server::MapServer::mapCallback, this);
+
+  // Change the currently published map
+  change_map_srv_ = nh_.advertiseService("change_map", &map_server::MapServer::changeMapCallback, this);
+
+  // Latched publisher for metadata
+  metadata_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+
+  // Latched publisher for data
+  map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
+  if (type_ != 8) map_convert_ = nh_.advertise<nav_msgs::OccupancyGrid>("map_convert", 1, true);
+
+  deprecated_ = (res != 0);
+  if (!deprecated_)
+  {
+    if (!loadMapFromYaml(fname))
+    {
+      exit(-1);
+    }
+  }
+  else
+  {
+    if (!loadMapFromParams(fname, res))
+    {
+      exit(-1);
+    }
+  }
+}
+
+bool map_server::MapServer::mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
+{
+  // request is empty; we ignore it
+
+  // = operator is overloaded to make deep copy (tricky!)
+  res = map_resp_;
+  ROS_INFO("Sending map");
+
+  return true;
+}
+
+bool map_server::MapServer::changeMapCallback(nav_msgs::LoadMap::Request& request,
+  nav_msgs::LoadMap::Response& response)
+{
+  if (loadMapFromYaml(request.map_url))
+  {
+    response.result = response.RESULT_SUCCESS;
+    ROS_INFO("Changed map to %s", request.map_url.c_str());
+  }
+  else
+  {
+    response.result = response.RESULT_UNDEFINED_FAILURE;
+  }
+  return true;
+}
+
+
+bool map_server::MapServer::loadMapFromValues(std::string map_file_name, double resolution,
+  int negate, double occ_th, double free_th,
+  double origin[3], MapMode mode)
+{
+  ROS_INFO("Loading map from images \"%s\"", map_file_name.c_str());
+  try
+  {
+    if (type_ == 32)
+      map_server::loadMapFromFile32Bit(&map_resp_, map_file_name.c_str(),
+        resolution, negate, occ_th, free_th,
+        origin, mode);
+    else if (type_ == 16)
+      map_server::loadMapFromFile16Bit(&map_resp_, map_file_name.c_str(),
+        resolution, negate, occ_th, free_th,
+        origin, mode);
+    else
+      map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(),
+        resolution, negate, occ_th, free_th,
+        origin, mode);
+  }
+  catch (std::runtime_error& e)
+  {
+    ROS_ERROR("%s", e.what());
+    return false;
+  }
+
+  // To make sure get a consistent time in simulation
+  ros::Time::waitForValid();
+  map_resp_.map.info.map_load_time = ros::Time::now();
+  map_resp_.map.header.frame_id = frame_id_;
+  map_resp_.map.header.stamp = ros::Time::now();
+  ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
+    map_resp_.map.info.width,
+    map_resp_.map.info.height,
+    map_resp_.map.info.resolution);
+  meta_data_message_ = map_resp_.map.info;
+
+  // Publish latched topics
+  metadata_pub_.publish(meta_data_message_);
+  map_pub_.publish(map_resp_.map);
+
+  if (type_ == 16)
+  {
+    double occ_th = 0.65;
+    double free_th = 0.196;
+    nav_msgs::OccupancyGrid convert;
+    convert.header = map_resp_.map.header;
+    convert.info = map_resp_.map.info;
+    convert.info.width = map_resp_.map.info.width / 2;
+    convert.data.resize(convert.info.width * convert.info.height);
+    unsigned int color_sum, color;
+    double color_avg;
+    unsigned char value;
+    int size_y = map_resp_.map.info.height;
+    int size_x = map_resp_.map.info.width;
+
+    for (int j = 0; j < map_resp_.map.info.height; j++)
+    {
+      for (int i = 0; i < map_resp_.map.info.width; i += 2)
+      {
+        unsigned char low = map_resp_.map.data[MAP_IDX(size_x, i, size_y - j - 1)];
+        unsigned char high = map_resp_.map.data[MAP_IDX(size_x, i + 1, size_y - j - 1)];
+        color = (int)((low & 0x00FF) | (high << 8));
+        unsigned char red = (unsigned char)(color & 0x00FF0000) >> 16;
+        unsigned char green = (unsigned char)(color & 0x0000FF00) >> 8;
+        unsigned char blue = (unsigned char)(color & 0x000000FF);
+        color_sum = red + green + blue;
+
+        color_avg = color_sum / 2.0;
+
+        double occ = (255 - color_avg) / 255.0;
+        if (occ > occ_th)
+          value = +100;
+        else if (occ < free_th)
+          value = 0;
+        else
+        {
+          double ratio = (occ - free_th) / (occ_th - free_th);
+          value = 1 + 98 * ratio;
+        }
+        convert.data[MAP_IDX(size_x / 2, i / 2, size_y - j - 1)] = value;
+      }
+    }
+    map_convert_.publish(convert);
+  }
+  return true;
+}
+
+
+bool map_server::MapServer::loadMapFromParams(std::string map_file_name, double resolution)
+{
+  ros::NodeHandle private_nh("~");
+  int negate;
+  double occ_th;
+  double free_th;
+  double origin[3];
+  private_nh.param("negate", negate, 0);
+  private_nh.param("occupied_thresh", occ_th, 0.65);
+  private_nh.param("free_thresh", free_th, 0.196);
+  origin[0] = origin[1] = origin[2] = 0.0;
+  return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY);
+}
+
+
+bool map_server::MapServer::loadMapFromYaml(std::string path_to_yaml)
+{
+  std::string mapfname;
+  MapMode mode;
+  double res;
+  int negate;
+  double occ_th;
+  double free_th;
+  double origin[3];
+  std::ifstream fin(path_to_yaml.c_str());
+  if (fin.fail())
+  {
+    ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
+    return false;
+  }
+#ifdef HAVE_YAMLCPP_GT_0_5_0
+  // The document loading process changed in yaml-cpp 0.5.
+  YAML::Node doc = YAML::Load(fin);
+#else
+  YAML::Parser parser(fin);
+  YAML::Node doc;
+  parser.GetNextDocument(doc);
+#endif
+  try
+  {
+    doc["resolution"] >> res;
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
+    return false;
+  }
+  try
+  {
+    doc["negate"] >> negate;
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain a negate tag or it is invalid.");
+    return false;
+  }
+  try
+  {
+    doc["occupied_thresh"] >> occ_th;
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
+    return false;
+  }
+  try
+  {
+    doc["free_thresh"] >> free_th;
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
+    return false;
+  }
+  try
+  {
+    std::string modeS = "";
+    doc["mode"] >> modeS;
+
+    if (modeS == "trinary")
+      mode = TRINARY;
+    else if (modeS == "scale")
+      mode = SCALE;
+    else if (modeS == "raw")
+      mode = RAW;
+    else
+    {
+      ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
+      return false;
+    }
+  }
+  catch (YAML::Exception&)
+  {
+    ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
+    mode = TRINARY;
+  }
+  try
+  {
+    doc["origin"][0] >> origin[0];
+    doc["origin"][1] >> origin[1];
+    doc["origin"][2] >> origin[2];
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain an origin tag or it is invalid.");
+    return false;
+  }
+  try
+  {
+    doc["image"] >> mapfname;
+    // TODO: make this path-handling more robust
+    if (mapfname.size() == 0)
+    {
+      ROS_ERROR("The image tag cannot be an empty string.");
+      return false;
+    }
+
+    boost::filesystem::path mapfpath(mapfname);
+    if (!mapfpath.is_absolute())
+    {
+      boost::filesystem::path dir(path_to_yaml);
+      dir = dir.parent_path();
+      mapfpath = dir / mapfpath;
+      mapfname = mapfpath.string();
+    }
+  }
+  catch (YAML::InvalidScalar&)
+  {
+    ROS_ERROR("The map does not contain an image tag or it is invalid.");
+    return false;
+  }
+  return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode);
+}
+
+
+
--- a/Localizations/Libraries/Ros/map_server/src/map_server.dox
+++ b/Localizations/Libraries/Ros/map_server/src/map_server.dox
@@ -0,0 +1,9 @@
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+Command-line usage is in the wiki.
+*/
+
--- a/Localizations/Libraries/Ros/map_server/test/consumer.py
+++ b/Localizations/Libraries/Ros/map_server/test/consumer.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# 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 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.
+#
+
+from __future__ import print_function
+
+PKG = 'static_map_server'
+NAME = 'consumer'
+
+import sys
+import unittest
+import time
+
+import rospy
+import rostest
+from nav_msgs.srv import GetMap
+
+
+class TestConsumer(unittest.TestCase):
+    def __init__(self, *args):
+        super(TestConsumer, self).__init__(*args)
+        self.success = False
+
+    def callback(self, data):
+        print(rospy.get_caller_id(), "I heard %s" % data.data)
+        self.success = data.data and data.data.startswith('hello world')
+        rospy.signal_shutdown('test done')
+
+    def test_consumer(self):
+        rospy.wait_for_service('static_map')
+        mapsrv = rospy.ServiceProxy('static_map', GetMap)
+        resp = mapsrv()
+        self.success = True
+        print(resp)
+        while not rospy.is_shutdown() and not self.success:  # and time.time() < timeout_t: <== timeout_t doesn't exists??
+            time.sleep(0.1)
+        self.assert_(self.success)
+        rospy.signal_shutdown('test done')
+
+if __name__ == '__main__':
+    rostest.rosrun(PKG, NAME, TestConsumer, sys.argv)
--- a/Localizations/Libraries/Ros/map_server/test/rtest.cpp
+++ b/Localizations/Libraries/Ros/map_server/test/rtest.cpp
@@ -0,0 +1,198 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <ros/service.h>
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
+#include <nav_msgs/LoadMap.h>
+
+#include "test_constants.h"
+
+int g_argc;
+char** g_argv;
+
+class MapClientTest : public testing::Test
+{
+  public:
+    // A node is needed to make a service call
+    ros::NodeHandle* n_;
+
+    MapClientTest()
+    {
+      ros::init(g_argc, g_argv, "map_client_test");
+      n_ = new ros::NodeHandle();
+      got_map_ = false;
+      got_map_metadata_ = false;
+    }
+
+    ~MapClientTest()
+    {
+      delete n_;
+    }
+
+    bool got_map_;
+    boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
+    void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
+    {
+      map_ = map;
+      got_map_ = true;
+    }
+
+    bool got_map_metadata_;
+    boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
+    void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
+    {
+      map_metadata_ = map_metadata;
+      got_map_metadata_ = true;
+    }
+};
+
+/* Try to retrieve the map via service, and compare to ground truth */
+TEST_F(MapClientTest, call_service)
+{
+  nav_msgs::GetMap::Request  req;
+  nav_msgs::GetMap::Response resp;
+  ASSERT_TRUE(ros::service::waitForService("static_map", 5000));
+  ASSERT_TRUE(ros::service::call("static_map", req, resp));
+  ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
+  ASSERT_EQ(resp.map.info.width, g_valid_image_width);
+  ASSERT_EQ(resp.map.info.height, g_valid_image_height);
+  ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map");
+  for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
+    ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
+}
+
+/* Try to retrieve the map via topic, and compare to ground truth */
+TEST_F(MapClientTest, subscribe_topic)
+{
+  ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, [this](auto& map){ mapCallback(map); });
+
+  // Try a few times, because the server may not be up yet.
+  int i=20;
+  while(!got_map_ && i > 0)
+  {
+    ros::spinOnce();
+    ros::Duration d = ros::Duration().fromSec(0.25);
+    d.sleep();
+    i--;
+  }
+  ASSERT_TRUE(got_map_);
+  ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res);
+  ASSERT_EQ(map_->info.width, g_valid_image_width);
+  ASSERT_EQ(map_->info.height, g_valid_image_height);
+  ASSERT_STREQ(map_->header.frame_id.c_str(), "map");
+  for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
+    ASSERT_EQ(g_valid_image_content[i], map_->data[i]);
+}
+
+/* Try to retrieve the metadata via topic, and compare to ground truth */
+TEST_F(MapClientTest, subscribe_topic_metadata)
+{
+  ros::Subscriber sub = n_->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, [this](auto& map_metadata){ mapMetaDataCallback(map_metadata); });
+
+  // Try a few times, because the server may not be up yet.
+  int i=20;
+  while(!got_map_metadata_ && i > 0)
+  {
+    ros::spinOnce();
+    ros::Duration d = ros::Duration().fromSec(0.25);
+    d.sleep();
+    i--;
+  }
+  ASSERT_TRUE(got_map_metadata_);
+  ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res);
+  ASSERT_EQ(map_metadata_->width, g_valid_image_width);
+  ASSERT_EQ(map_metadata_->height, g_valid_image_height);
+}
+
+/* Change the map, retrieve the map via topic, and compare to ground truth */
+TEST_F(MapClientTest, change_map)
+{
+  ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, [this](auto& map){ mapCallback(map); });
+
+  // Try a few times, because the server may not be up yet.
+  for (int i = 20; i > 0 && !got_map_; i--)
+  {
+    ros::spinOnce();
+    ros::Duration d = ros::Duration().fromSec(0.25);
+    d.sleep();
+  }
+  ASSERT_TRUE(got_map_);
+
+  // Now change the map
+  got_map_ = false;
+  nav_msgs::LoadMap::Request  req;
+  nav_msgs::LoadMap::Response resp;
+  req.map_url = ros::package::getPath("map_server") + "/test/testmap2.yaml";
+  ASSERT_TRUE(ros::service::waitForService("change_map", 5000));
+  ASSERT_TRUE(ros::service::call("change_map", req, resp));
+  ASSERT_EQ(0u, resp.result);
+  for (int i = 20; i > 0 && !got_map_; i--)
+  {
+    ros::spinOnce();
+    ros::Duration d = ros::Duration().fromSec(0.25);
+    d.sleep();
+  }
+
+  ASSERT_FLOAT_EQ(tmap2::g_valid_image_res, map_->info.resolution);
+  ASSERT_EQ(tmap2::g_valid_image_width, map_->info.width);
+  ASSERT_EQ(tmap2::g_valid_image_height, map_->info.height);
+  ASSERT_STREQ("map", map_->header.frame_id.c_str());
+  for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
+    ASSERT_EQ(tmap2::g_valid_image_content[i], map_->data[i]) << "idx:" << i;
+
+  //Put the old map back so the next test isn't broken
+  got_map_ = false;
+  req.map_url = ros::package::getPath("map_server") + "/test/testmap.yaml";
+  ASSERT_TRUE(ros::service::call("change_map", req, resp));
+  ASSERT_EQ(0u, resp.result);
+  for (int i = 20; i > 0 && !got_map_; i--)
+  {
+    ros::spinOnce();
+    ros::Duration d = ros::Duration().fromSec(0.25);
+    d.sleep();
+  }
+  ASSERT_TRUE(got_map_);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  g_argc = argc;
+  g_argv = argv;
+
+  return RUN_ALL_TESTS();
+}
--- a/Localizations/Libraries/Ros/map_server/test/rtest.xml
+++ b/Localizations/Libraries/Ros/map_server/test/rtest.xml
@@ -0,0 +1,22 @@
+<launch>
+
+  <arg name="map_file" default="$(find map_server)/maps/maze.yaml" doc="Path to a map .yaml file (required)." />
+  <arg name="virtual_walls_map_file" default="$(find map_server)/maps/maze_virtual_walls.yaml" doc="Path to a virtual walls map .yaml file (optional)." />
+  <arg name="with_virtual_walls" default="false" />
+
+  <node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/lane_mask" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+    <param name="type" type="int" value="16"/>
+  </node>
+
+  <!-- <node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+  </node> -->
+  <!-- <arg name="direction_zones_map_file" default="$(find mir_gazebo)/maps/warehouse/direction_zones.yaml" doc="Path to a direction zones .yaml file (optional)." />
+  <arg name="with_direction_zones" default="true" />
+  <node if="$(eval direction_zones_map_file != '')" name="direction_zones_map_server" pkg="map_server" type="map_server" args="$(arg direction_zones_map_file)" ns="/direction_zones" output="screen">
+    <param name="frame_id" type="string" value="map"/>
+    <param name="type" type="int" value="16"/>
+  </node> -->
+
+</launch>
--- a/Localizations/Libraries/Ros/map_server/test/spectrum.png
+++ b/Localizations/Libraries/Ros/map_server/test/spectrum.png
--- a/Localizations/Libraries/Ros/map_server/test/test_constants.cpp
+++ b/Localizations/Libraries/Ros/map_server/test/test_constants.cpp
@@ -0,0 +1,84 @@
+/*
+ * 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 */
+
+/* This file contains global constants shared among tests */
+
+/* Note that these must be changed if the test image changes */
+
+#include "test_constants.h"
+
+const unsigned int g_valid_image_width = 10;
+const unsigned int g_valid_image_height = 10;
+// Note that the image content is given in row-major order, with the
+// lower-left pixel first.  This is different from a graphics coordinate
+// system, which starts with the upper-left pixel.  The loadMapFromFile
+// call converts from the latter to the former when it loads the image, and
+// we want to compare against the result of that conversion.
+const char g_valid_image_content[] = {
+0,0,0,0,0,0,0,0,0,0,
+100,100,100,100,0,0,100,100,100,0,
+100,100,100,100,0,0,100,100,100,0,
+100,0,0,0,0,0,0,0,0,0,
+100,0,0,0,0,0,0,0,0,0,
+100,0,0,0,0,0,100,100,0,0,
+100,0,0,0,0,0,100,100,0,0,
+100,0,0,0,0,0,100,100,0,0,
+100,0,0,0,0,0,100,100,0,0,
+100,0,0,0,0,0,0,0,0,0,
+};
+
+const char* g_valid_png_file = "test/testmap.png";
+const char* g_valid_bmp_file = "test/testmap.bmp";
+const char* g_spectrum_png_file = "test/spectrum.png";
+
+const float g_valid_image_res = 0.1;
+
+// Constants for testmap2
+namespace tmap2
+{
+  const char g_valid_image_content[] = {
+  100,100,100,100,100,100,100,100,100,100,100,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,100,100,100,100,100,100,100,100,0,100,
+  100,0,100,0,0,0,0,0,0,100,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,0,100,0,0,0,0,0,0,100,0,100,
+  100,0,0,0,0,0,0,0,0,0,0,100,
+  100,100,100,100,100,100,100,100,100,100,100,100,
+  };
+  const float g_valid_image_res = 0.1;
+  const unsigned int g_valid_image_width = 12;
+  const unsigned int g_valid_image_height = 12;
+}
--- a/Localizations/Libraries/Ros/map_server/test/test_constants.h
+++ b/Localizations/Libraries/Ros/map_server/test/test_constants.h
@@ -0,0 +1,51 @@
+/*
+ * 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.
+ */
+#ifndef MAP_SERVER_TEST_CONSTANTS_H
+#define MAP_SERVER_TEST_CONSTANTS_H
+
+/* Author: Brian Gerkey */
+
+/* This file externs global constants shared among tests */
+
+extern const unsigned int g_valid_image_width;
+extern const unsigned int g_valid_image_height;
+extern const char g_valid_image_content[];
+extern const char* g_valid_png_file;
+extern const char* g_valid_bmp_file;
+extern const float g_valid_image_res;
+extern const char* g_spectrum_png_file;
+
+namespace tmap2 {
+  extern const char g_valid_image_content[];
+  extern const float g_valid_image_res;
+  extern const unsigned int g_valid_image_width;
+  extern const unsigned int g_valid_image_height;
+}
+
+#endif
--- a/Localizations/Libraries/Ros/map_server/test/testmap.bmp
+++ b/Localizations/Libraries/Ros/map_server/test/testmap.bmp
--- a/Localizations/Libraries/Ros/map_server/test/testmap.png
+++ b/Localizations/Libraries/Ros/map_server/test/testmap.png
--- a/Localizations/Libraries/Ros/map_server/test/testmap.yaml
+++ b/Localizations/Libraries/Ros/map_server/test/testmap.yaml
@@ -0,0 +1,6 @@
+image: testmap.png
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
--- a/Localizations/Libraries/Ros/map_server/test/testmap2.png
+++ b/Localizations/Libraries/Ros/map_server/test/testmap2.png
--- a/Localizations/Libraries/Ros/map_server/test/testmap2.yaml
+++ b/Localizations/Libraries/Ros/map_server/test/testmap2.yaml
@@ -0,0 +1,6 @@
+image: testmap2.png
+resolution: 0.1
+origin: [-2.0, -3.0, -1.0]
+negate: 1
+occupied_thresh: 0.5
+free_thresh: 0.2
--- a/Localizations/Libraries/Ros/map_server/test/utest.cpp
+++ b/Localizations/Libraries/Ros/map_server/test/utest.cpp
@@ -0,0 +1,149 @@
+/*
+ * 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 <stdexcept> // for std::runtime_error
+#include <gtest/gtest.h>
+#include "map_server/image_loader.h"
+#include "test_constants.h"
+
+/* Try to load a valid PNG file.  Succeeds if no exception is thrown, and if
+ * the loaded image matches the known dimensions and content of the file.
+ *
+ * This test can fail on OS X, due to an apparent limitation of the
+ * underlying SDL_Image library. */
+TEST(MapServer, loadValidPNG)
+{
+  try
+  {
+    nav_msgs::GetMap::Response map_resp;
+    double origin[3] = { 0.0, 0.0, 0.0 };
+    map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1, origin);
+    EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
+    EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
+    EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
+    for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++)
+      EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+  }
+  catch(...)
+  {
+    ADD_FAILURE() << "Uncaught exception : " << "This is OK on OS X";
+  }
+}
+
+/* Try to load a valid BMP file.  Succeeds if no exception is thrown, and if
+ * the loaded image matches the known dimensions and content of the file. */
+TEST(MapServer, loadValidBMP)
+{
+  try
+  {
+    nav_msgs::GetMap::Response map_resp;
+    double origin[3] = { 0.0, 0.0, 0.0 };
+    map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false, 0.65, 0.1, origin);
+    EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
+    EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
+    EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
+    for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++)
+      EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+  }
+  catch(...)
+  {
+    ADD_FAILURE() << "Uncaught exception";
+  }
+}
+
+/* Try to load an invalid file.  Succeeds if a std::runtime_error exception
+ * is thrown. */
+TEST(MapServer, loadInvalidFile)
+{
+  try
+  {
+    nav_msgs::GetMap::Response map_resp;
+    double origin[3] = { 0.0, 0.0, 0.0 };
+    map_server::loadMapFromFile(&map_resp, "foo", 0.1, false, 0.65, 0.1, origin);
+  }
+  catch(std::runtime_error &e)
+  {
+    SUCCEED();
+    return;
+  }
+  catch(...)
+  {
+    FAIL() << "Uncaught exception";
+  }
+  ADD_FAILURE() << "Didn't throw exception as expected";
+}
+
+std::vector<unsigned int> countValues(const nav_msgs::GetMap::Response& map_resp)
+{
+  std::vector<unsigned int> counts(256, 0);
+  for (unsigned int i = 0; i < map_resp.map.data.size(); i++)
+  {
+    unsigned char value = static_cast<unsigned char>(map_resp.map.data[i]);
+    counts[value]++;
+  }
+  return counts;
+}
+
+TEST(MapServer, testMapMode)
+{
+  nav_msgs::GetMap::Response map_resp;
+  double origin[3] = { 0.0, 0.0, 0.0 };
+
+  map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, TRINARY);
+  std::vector<unsigned int> trinary_counts = countValues(map_resp);
+  EXPECT_EQ(90u, trinary_counts[100]);
+  EXPECT_EQ(26u, trinary_counts[0]);
+  EXPECT_EQ(140u, trinary_counts[255]);
+
+  map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, SCALE);
+  std::vector<unsigned int> scale_counts = countValues(map_resp);
+  EXPECT_EQ(90u, scale_counts[100]);
+  EXPECT_EQ(26u, scale_counts[0]);
+  unsigned int scaled_values = 0;
+  for (unsigned int i = 1; i < 100; i++)
+  {
+    scaled_values += scale_counts[i];
+  }
+  EXPECT_EQ(140u, scaled_values);
+
+  map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, RAW);
+  std::vector<unsigned int> raw_counts = countValues(map_resp);
+  for (unsigned int i = 0; i < raw_counts.size(); i++)
+  {
+    EXPECT_EQ(1u, raw_counts[i]) << i;
+  }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}