git commit -m "first commit for v2"

Localizations/Libraries/Ros/hector_slam/.gitignore

@@ -0,0 +1,10 @@
+bin/
+build/
+lib/
+msg_gen/
+srv_gen/
+hector_mapping/src/hector_mapping/
+hector_nav_msgs/src/hector_nav_msgs/
+.idea/
+.vscode/
+cmake-build-*/

Localizations/Libraries/Ros/hector_slam/README.txt

@@ -0,0 +1,3 @@
+# hector_slam
+
+See the ROS Wiki for documentation: http://wiki.ros.org/hector_slam

Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CHANGELOG.rst

@@ -0,0 +1,50 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_compressed_map_transport
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+* Fix out of bounds access
+  Fixes `#52 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/52>`_ and `#70 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/70>`_
+* Contributors: Stefan Fabian
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+* Use the FindEigen3.cmake module provided by Eigen
+* Contributors: Johannes Meyer
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+* fixed cmake find for eigen in indigo
+* fixed libopencv-dev rosdep key
+* Contributors: Alexander Stumpf, Johannes Meyer
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_compressed_map_transport)
+
+find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs hector_map_tools image_transport nav_msgs sensor_msgs)
+
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs sensor_msgs
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(map_to_image_node src/map_to_image_node.cpp)
+target_link_libraries(map_to_image_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS map_to_image_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_compressed_map_transport</name>
+  <version>0.5.2</version>
+  <description>
+    hector_compressed_map_transport provides means for transporting compressed map data through the use of image_transport.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://ros.org/wiki/hector_compressed_map_transport</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>hector_map_tools</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>eigen</build_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>hector_map_tools</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>eigen</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/src/map_to_image_node.cpp

@@ -0,0 +1,270 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 <nav_msgs/GetMap.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <Eigen/Geometry>
+
+#include <hector_map_tools/HectorMapTools.h>
+
+using namespace std;
+
+/**
+ * @brief This node provides occupancy grid maps as images via image_transport, so the transmission consumes less bandwidth.
+ * The provided code is a incomplete proof of concept.
+ */
+class MapAsImageProvider
+{
+public:
+  MapAsImageProvider()
+    : pn_("~")
+  {
+
+    image_transport_ = new image_transport::ImageTransport(n_);
+    image_transport_publisher_full_ = image_transport_->advertise("map_image/full", 1);
+    image_transport_publisher_tile_ = image_transport_->advertise("map_image/tile", 1);
+
+    pose_sub_ = n_.subscribe("pose", 1, &MapAsImageProvider::poseCallback, this);
+    map_sub_ = n_.subscribe("map", 1, &MapAsImageProvider::mapCallback, this);
+
+    //Which frame_id makes sense?
+    cv_img_full_.header.frame_id = "map_image";
+    cv_img_full_.encoding = sensor_msgs::image_encodings::MONO8;
+
+    cv_img_tile_.header.frame_id = "map_image";
+    cv_img_tile_.encoding = sensor_msgs::image_encodings::MONO8;
+
+    //Fixed cell width for tile based image, use dynamic_reconfigure for this later
+    p_size_tiled_map_image_x_ = 64;
+    p_size_tiled_map_image_y_ = 64;
+
+    ROS_INFO("Map to Image node started.");
+  }
+
+  ~MapAsImageProvider()
+  {
+    delete image_transport_;
+  }
+
+  //We assume the robot position is available as a PoseStamped here (querying tf would be the more general option)
+  void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose)
+  {
+    pose_ptr_ = pose;
+  }
+
+  //The map->image conversion runs every time a new map is received at the moment
+  void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
+  {
+    int size_x = map->info.width;
+    int size_y = map->info.height;
+
+    if ((size_x < 3) || (size_y < 3) ){
+      ROS_INFO("Map size is only x: %d,  y: %d . Not running map to image conversion", size_x, size_y);
+      return;
+    }
+
+    // Only if someone is subscribed to it, do work and publish full map image
+    if (image_transport_publisher_full_.getNumSubscribers() > 0){
+      cv::Mat* map_mat  = &cv_img_full_.image;
+
+      // resize cv image if it doesn't have the same dimensions as the map
+      if ( (map_mat->rows != size_y) && (map_mat->cols != size_x)){
+        *map_mat = cv::Mat(size_y, size_x, CV_8U);
+      }
+
+      const std::vector<int8_t>& map_data (map->data);
+
+      unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
+
+      //We have to flip around the y axis, y for image starts at the top and y for map at the bottom
+      int size_y_rev = size_y-1;
+
+      for (int y = size_y_rev; y >= 0; --y){
+
+        int idx_map_y = size_x * (size_y_rev - y);
+        int idx_img_y = size_x * y;
+
+        for (int x = 0; x < size_x; ++x){
+
+          int idx = idx_img_y + x;
+
+          switch (map_data[idx_map_y + x])
+          {
+          case -1:
+            map_mat_data_p[idx] = 127;
+            break;
+
+          case 0:
+            map_mat_data_p[idx] = 255;
+            break;
+
+          case 100:
+            map_mat_data_p[idx] = 0;
+            break;
+          }
+        }
+      }
+      image_transport_publisher_full_.publish(cv_img_full_.toImageMsg());
+    }
+
+    // Only if someone is subscribed to it, do work and publish tile-based map image Also check if pose_ptr_ is valid
+    if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){
+
+      world_map_transformer_.setTransforms(*map);
+
+      Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
+      Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
+
+      Eigen::Vector2i rob_position_mapi (rob_position_map.cast<int>());
+
+      Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2);
+
+      Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi);
+
+      //Clamp to lower map coords
+      if (min_coords_map[0] < 0){
+        min_coords_map[0] = 0;
+      }
+
+      if (min_coords_map[1] < 0){
+        min_coords_map[1] = 0;
+      }
+
+      Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_));
+
+      //Clamp to upper map coords
+      if (max_coords_map[0] > size_x){
+
+        int diff = max_coords_map[0] - size_x;
+        min_coords_map[0] -= diff;
+
+        max_coords_map[0] = size_x;
+      }
+
+      if (max_coords_map[1] > size_y){
+
+        int diff = max_coords_map[1] - size_y;
+        min_coords_map[1] -= diff;
+
+        max_coords_map[1] = size_y;
+      }
+
+      //Clamp lower again (in case the map is smaller than the selected visualization window)
+      if (min_coords_map[0] < 0){
+        min_coords_map[0] = 0;
+      }
+
+      if (min_coords_map[1] < 0){
+        min_coords_map[1] = 0;
+      }
+
+      Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map);
+
+      cv::Mat* map_mat  = &cv_img_tile_.image;
+
+      // resize cv image if it doesn't have the same dimensions as the selected visualization window
+      if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){
+        *map_mat = cv::Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U);
+      }
+
+      const std::vector<int8_t>& map_data (map->data);
+
+      unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
+
+      //We have to flip around the y axis, y for image starts at the top and y for map at the bottom
+      int y_img = max_coords_map[1]-1;
+
+      for (int y = min_coords_map[1]; y < max_coords_map[1];++y){
+
+        int idx_map_y = y_img-- * size_x;
+        int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x();
+
+        for (int x = min_coords_map[0]; x < max_coords_map[0];++x){
+
+          int img_index = idx_img_y + (x-min_coords_map[0]);
+
+          switch (map_data[idx_map_y+x])
+          {
+          case 0:
+            map_mat_data_p[img_index] = 255;
+            break;
+
+          case -1:
+            map_mat_data_p[img_index] = 127;
+            break;
+
+          case 100:
+            map_mat_data_p[img_index] = 0;
+            break;
+          }
+        }        
+      }
+      image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg());
+    }
+  }
+
+  ros::Subscriber map_sub_;
+  ros::Subscriber pose_sub_;
+
+  image_transport::Publisher image_transport_publisher_full_;
+  image_transport::Publisher image_transport_publisher_tile_;
+
+  image_transport::ImageTransport* image_transport_;
+
+  geometry_msgs::PoseStampedConstPtr pose_ptr_;
+
+  cv_bridge::CvImage cv_img_full_;
+  cv_bridge::CvImage cv_img_tile_;
+
+  ros::NodeHandle n_;
+  ros::NodeHandle pn_;
+
+  int p_size_tiled_map_image_x_;
+  int p_size_tiled_map_image_y_;
+
+  HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
+
+};
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "map_to_image_node");
+
+  MapAsImageProvider map_image_provider;
+
+  ros::spin();
+
+  return 0;
+}

Localizations/Libraries/Ros/hector_slam/hector_geotiff/CHANGELOG.rst

@@ -0,0 +1,68 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_geotiff
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+* Refactored hector_geotiff dependencies.
+* Contributors: Stefan Fabian
+
+0.5.1 (2021-01-15)
+------------------
+* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node.
+  Some minor cleanup.
+* Contributors: Stefan Fabian
+
+0.5.0 (2020-12-17)
+------------------
+* Added missing dependency for Qt5 cmake.
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Qt5 support for hector geotiff on headless systems.
+* Updated platform args. Test on robot.
+* Experiments with platform argument.
+* Renamed depends for (hopefully soon) rosdep compatibility.
+* Moved to Qt5.
+* Contributors: Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+* Update geotiff draw interface to support different shapes
+* Contributors: Stefan Kohlbrecher
+
+0.3.5 (2016-06-24)
+------------------
+* Use the FindEigen3.cmake module provided by Eigen
+* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths
+* Contributors: Dorothea Koert, Johannes Meyer
+
+0.3.4 (2015-11-07)
+------------------
+* Removes trailing spaces and fixes indents
+* Contributors: YuK_Ota
+
+0.3.3 (2014-06-15)
+------------------
+* fixed cmake find for eigen in indigo
+* added launchfile to restart geotiff node
+* Contributors: Alexander Stumpf
+
+0.3.2 (2014-03-30)
+------------------
+* Add TrajectoryMapWriter to geotiff_mapper.launch per default
+* Add arguments to launch files for specifying geotiff file path
+* Contributors: Stefan Kohlbrecher
+
+0.3.1 (2013-10-09)
+------------------
+* added missing install rule for launch files
+* moved header files from include/geotiff_writer to include/hector_geotiff
+* fixed warnings for deprecated pluginlib method/macro calls
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_geotiff/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_geotiff)
+set(CMAKE_CXX_STANDARD 14)
+
+find_package(catkin REQUIRED COMPONENTS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs)
+
+find_package(Qt5 COMPONENTS Widgets REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES geotiff_writer
+  CATKIN_DEPENDS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs
+  DEPENDS EIGEN3 Qt5Widgets
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${Qt5Widgets_INCLUDE_DIRS}
+)
+
+add_library(geotiff_writer src/geotiff_writer/geotiff_writer.cpp)
+target_link_libraries(geotiff_writer ${catkin_LIBRARIES} Qt5::Widgets stdc++fs)
+add_dependencies(geotiff_writer ${catkin_EXPORTED_TARGETS})
+
+add_executable(geotiff_saver src/geotiff_saver.cpp)
+target_link_libraries(geotiff_saver geotiff_writer)
+add_dependencies(geotiff_saver ${catkin_EXPORTED_TARGETS})
+
+add_executable(geotiff_node src/geotiff_node.cpp)
+target_link_libraries(geotiff_node geotiff_writer)
+add_dependencies(geotiff_node ${catkin_EXPORTED_TARGETS})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS geotiff_writer geotiff_saver geotiff_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY fonts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+

Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/LICENSE.txt

@@ -0,0 +1,203 @@
+License for Roboto-Regular.ttf:
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/geotiff_writer.h

@@ -0,0 +1,139 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _GEOTIFFWRITER_H__
+#define _GEOTIFFWRITER_H__
+
+#include "map_writer_interface.h"
+
+#include <Eigen/Geometry>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
+
+#include <QImage>
+#include <QApplication>
+#include <QFont>
+
+#include <hector_map_tools/HectorMapTools.h>
+
+#if  __cplusplus < 201703L
+	#include <experimental/filesystem>
+	namespace fs = std::experimental::filesystem;
+#else
+	#include <filesystem>
+	namespace fs = std::filesystem;
+#endif
+
+
+namespace hector_geotiff{
+
+
+class GeotiffWriter : public MapWriterInterface
+{
+  public:
+  explicit GeotiffWriter(bool useCheckerboardCacheIn = false);
+  virtual ~GeotiffWriter();
+
+  //setUsePrecalcGrid(bool usePrecalc, const Eigen::Vector2f& size);
+
+  void setMapFileName(const std::string& mapFileName);
+  void setMapFilePath(const std::string& mapFilePath);
+  void setUseUtcTimeSuffix(bool useSuffix);
+
+  void setupImageSize();
+  bool setupTransforms(const nav_msgs::OccupancyGrid& map);
+  void drawBackgroundCheckerboard();
+  void drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid = true);
+  void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape);
+  inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points){
+      drawPath(start, points, 120,0,240);
+  }
+  void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points, int color_r, int color_g, int color_b);
+  void drawCoords();
+  std::string getBasePathAndFileName() const;
+  void writeGeotiffImage(bool completed);
+
+
+protected:
+
+  void transformPainterToImgCoords(QPainter& painter);
+  void drawCross(QPainter& painter, const Eigen::Vector2f& coords);
+  void drawArrow(QPainter& painter);
+  void drawCoordSystem(QPainter& painter);
+
+  float resolution = std::numeric_limits<float>::quiet_NaN();
+  Eigen::Vector2f origin;
+
+  int resolutionFactor = 3;
+  float resolutionFactorf = std::numeric_limits<float>::quiet_NaN();
+
+  bool useCheckerboardCache;
+  bool use_utc_time_suffix_;
+
+  float pixelsPerMapMeter = std::numeric_limits<float>::quiet_NaN();
+  float pixelsPerGeoTiffMeter = std::numeric_limits<float>::quiet_NaN();
+
+  Eigen::Vector2i minCoordsMap;
+  Eigen::Vector2i maxCoordsMap;
+
+  Eigen::Vector2i sizeMap;
+  Eigen::Vector2f sizeMapf;
+
+  Eigen::Vector2f rightBottomMarginMeters;
+  Eigen::Vector2f rightBottomMarginPixelsf;
+  Eigen::Vector2i rightBottomMarginPixels;
+
+  Eigen::Vector2f leftTopMarginMeters;
+
+  Eigen::Vector2f totalMeters;
+
+  Eigen::Vector2i geoTiffSizePixels;
+
+  Eigen::Vector2f mapOrigInGeotiff;
+  Eigen::Vector2f mapEndInGeotiff;
+
+  std::string map_file_name_;
+  std::string map_file_path_;
+
+  QImage image;
+  QImage checkerboard_cache;
+  QApplication* app;
+  QString font_family_;
+  QFont map_draw_font_;
+
+  HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
+  HectorMapTools::CoordinateTransformer<float> map_geo_transformer_;
+  HectorMapTools::CoordinateTransformer<float> world_geo_transformer_;
+
+  nav_msgs::MapMetaData cached_map_meta_data_;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_interface.h

@@ -0,0 +1,64 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _MAPWRITERINTERFACE_H__
+#define _MAPWRITERINTERFACE_H__
+
+#include <vector>
+#include <Eigen/Core>
+
+namespace hector_geotiff{
+
+enum Shape {
+  SHAPE_CIRCLE,
+  SHAPE_DIAMOND
+};
+
+class MapWriterInterface{
+public:
+  struct Color {
+    Color(unsigned int r, unsigned int g, unsigned int b) : r(r), g(g), b(b) {}
+    unsigned int r,g,b;
+  };
+
+  bool completed_map_ = false;
+
+  virtual std::string getBasePathAndFileName() const = 0;
+  virtual void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape = SHAPE_CIRCLE) = 0;
+  //virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points) = 0;
+
+  inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points){
+      drawPath(start, points, 120,0,240);
+  }
+  virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points, int color_r, int color_g, int color_b) = 0;
+
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h

@@ -0,0 +1,47 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _MAPWRITERPLUGININTERFACE_H__
+#define _MAPWRITERPLUGININTERFACE_H__
+
+#include "map_writer_interface.h"
+
+namespace hector_geotiff{
+
+class MapWriterPluginInterface{
+
+public:
+
+  virtual void initialize(const std::string& name) = 0;
+  virtual void draw(MapWriterInterface* map_writer_interface) = 0;
+
+};
+
+} //namespace hector_geotiff
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_geotiff/package.xml

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hector_geotiff</name>
+  <version>0.5.2</version>
+  <description>
+    hector_geotiff provides a node that can be used to save occupancy grid map, robot trajectory and object of interest data to RoboCup Rescue compliant GeoTiff images.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_geotiff</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>qtbase5-dev</build_depend>
+  <build_export_depend>qtbase5-dev</build_export_depend>
+  <exec_depend>qt5-image-formats-plugins</exec_depend>
+  <depend>hector_map_tools</depend>
+  <depend>hector_nav_msgs</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_node.cpp

@@ -0,0 +1,325 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_geotiff/geotiff_writer.h"
+#include "hector_geotiff/map_writer_plugin_interface.h"
+
+#include <cstdio>
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <pluginlib/class_loader.h>
+
+#include <memory>
+#include <boost/algorithm/string.hpp>
+
+#include <geometry_msgs/Quaternion.h>
+#include <nav_msgs/GetMap.h>
+#include <std_msgs/String.h>
+#include <hector_nav_msgs/GetRobotTrajectory.h>
+
+#include <QApplication>
+
+using namespace std;
+
+namespace hector_geotiff{
+
+/**
+ * @brief Map generation node.
+ */
+class MapGenerator
+{
+public:
+  MapGenerator()
+    : geotiff_writer_(false)
+    , pn_("~")
+    , running_saved_map_num_(0)
+  {
+    pn_.param("map_file_path", p_map_file_path_, std::string("."));
+    geotiff_writer_.setMapFilePath(p_map_file_path_);
+    geotiff_writer_.setUseUtcTimeSuffix(true);
+
+    pn_.param("map_file_base_name", p_map_file_base_name_, std::string());
+
+    pn_.param("draw_background_checkerboard", p_draw_background_checkerboard_, true);
+    pn_.param("draw_free_space_grid", p_draw_free_space_grid_, true);
+
+    sys_cmd_sub_ = n_.subscribe("syscommand", 1, &MapGenerator::sysCmdCallback, this);
+    pn_.param("use_map_topic", use_map_topic_,false);
+
+    if(!use_map_topic_) map_service_client_ = n_.serviceClient<nav_msgs::GetMap>("map");
+    //object_service_client_ = n_.serviceClient<worldmodel_msgs::GetObjectModel>("worldmodel/get_object_model");
+    path_service_client_ = n_.serviceClient<hector_nav_msgs::GetRobotTrajectory>("trajectory");
+
+
+    double p_geotiff_save_period = 0.0;
+    pn_.param("geotiff_save_period", p_geotiff_save_period, 0.0);
+
+    if(p_geotiff_save_period > 0.0){
+      //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, false);
+      //publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false);
+      map_save_timer_ = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, this, false );
+    }
+
+
+    pn_.param("plugins", p_plugin_list_, std::string(""));
+
+    std::vector<std::string> plugin_list;
+    boost::algorithm::split(plugin_list, p_plugin_list_, boost::is_any_of("\t "));
+
+    //We always have at least one element containing "" in the string list
+    if ((plugin_list.size() > 0) && (plugin_list[0].length() > 0)){
+      plugin_loader_ = std::make_unique<pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>>("hector_geotiff", "hector_geotiff::MapWriterPluginInterface");
+
+      for (size_t i = 0; i < plugin_list.size(); ++i){
+        try
+        {
+          boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> tmp (plugin_loader_->createInstance(plugin_list[i]));
+          tmp->initialize(plugin_loader_->getName(plugin_list[i]));
+          plugin_vector_.push_back(tmp);
+        }
+        catch(pluginlib::PluginlibException& ex)
+        {
+          ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
+        }
+      }
+    }else{
+      ROS_INFO("No plugins loaded for geotiff node");
+    }
+
+    ROS_INFO("Geotiff node started");
+  }
+
+  ~MapGenerator() = default;
+
+  void writeGeotiff(bool completed)
+  {
+    ros::Time start_time (ros::Time::now());
+
+    std::stringstream ssStream;
+
+
+    bool received_map = false;
+    boost::shared_ptr<const nav_msgs::OccupancyGrid> map;
+    nav_msgs::GetMap srv_map;
+    if (use_map_topic_)
+    {
+      map = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("map",ros::Duration(4));
+      if (map != nullptr) received_map = true;
+    }
+    else
+    {
+      received_map = map_service_client_.call(srv_map);
+      map = boost::make_shared<nav_msgs::OccupancyGrid>(srv_map.response.map);
+    }
+    if (received_map)
+    {
+      ROS_INFO("GeotiffNode: Map service called successfully");
+
+      std::string map_file_name = p_map_file_base_name_;
+      std::string competition_name;
+      std::string team_name;
+      std::string mission_name;
+      std::string postfix;
+      if (n_.getParamCached("/competition", competition_name) && !competition_name.empty()) map_file_name = map_file_name + "_" + competition_name;
+      if (n_.getParamCached("/team", team_name)               && !team_name.empty())        map_file_name = map_file_name + "_" + team_name;
+      if (n_.getParamCached("/mission", mission_name)         && !mission_name.empty())     map_file_name = map_file_name + "_" + mission_name;
+      if (pn_.getParamCached("map_file_postfix", postfix)     && !postfix.empty())          map_file_name = map_file_name + "_" + postfix;
+      if (map_file_name.substr(0, 1) == "_") map_file_name = map_file_name.substr(1);
+      if (map_file_name.empty()) map_file_name = "GeoTiffMap";
+      geotiff_writer_.setMapFileName(map_file_name);
+      bool transformSuccess = geotiff_writer_.setupTransforms(*map);
+
+      if(!transformSuccess){
+        ROS_INFO("Couldn't set map transform");
+        return;
+      }
+
+      geotiff_writer_.setupImageSize();
+
+      if (p_draw_background_checkerboard_){
+        geotiff_writer_.drawBackgroundCheckerboard();
+      }
+
+      geotiff_writer_.drawMap(*map, p_draw_free_space_grid_);
+      geotiff_writer_.drawCoords();
+
+      geotiff_writer_.completed_map_ = completed;
+
+      //ROS_INFO("Sum: %ld", (long int)srv.response.sum);
+    }
+    else
+    {
+      ROS_ERROR("Failed to call map service");
+      return;
+    }
+
+    ROS_INFO("Writing geotiff plugins");
+    for (size_t i = 0; i < plugin_vector_.size(); ++i){
+      plugin_vector_[i]->draw(&geotiff_writer_);
+    }
+
+    ROS_INFO("Writing geotiff");
+
+    /**
+      * No Victims for now, first  agree on a common standard for representation
+      */
+    /*
+    if (req_object_model_){
+      worldmodel_msgs::GetObjectModel srv_objects;
+      if (object_service_client_.call(srv_objects))
+      {
+        ROS_INFO("GeotiffNode: Object service called successfully");
+
+        const worldmodel_msgs::ObjectModel& objects_model (srv_objects.response.model);
+
+        size_t size = objects_model.objects.size();
+
+
+        unsigned int victim_num  = 1;
+
+        for (size_t i = 0; i < size; ++i){
+          const worldmodel_msgs::Object& object (objects_model.objects[i]);
+
+          if (object.state.state == worldmodel_msgs::ObjectState::CONFIRMED){
+            geotiff_writer_.drawVictim(Eigen::Vector2f(object.pose.pose.position.x,object.pose.pose.position.y),victim_num);
+            victim_num++;
+          }
+        }
+      }
+      else
+      {
+        ROS_ERROR("Failed to call objects service");
+      }
+    }
+    */
+
+    /*
+    hector_nav_msgs::GetRobotTrajectory srv_path;
+
+    if (path_service_client_.call(srv_path))
+    {
+      ROS_INFO("GeotiffNode: Path service called successfully");
+
+      std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
+
+      size_t size = traj_vector.size();
+
+      std::vector<Eigen::Vector2f> pointVec;
+      pointVec.resize(size);
+
+      for (size_t i = 0; i < size; ++i){
+        const geometry_msgs::PoseStamped& pose (traj_vector[i]);
+
+        pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
+      }
+
+      if (size > 0){
+        //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
+        Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
+        geotiff_writer_.drawPath(startVec, pointVec);
+      }
+    }
+    else
+    {
+      ROS_ERROR("Failed to call path service");
+    }
+    */
+
+
+    geotiff_writer_.writeGeotiffImage(completed);
+    running_saved_map_num_++;
+
+    ros::Duration elapsed_time (ros::Time::now() - start_time);
+
+    ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec());
+  }
+
+  void timerSaveGeotiffCallback(const ros::TimerEvent& e)
+  {
+    this->writeGeotiff(false);
+  }
+
+  void sysCmdCallback(const std_msgs::String& sys_cmd)
+  {
+    if (sys_cmd.data != "savegeotiff"){
+      return;
+    }
+
+    this->writeGeotiff(true);
+  }
+
+  std::string p_map_file_path_;
+  std::string p_map_file_base_name_;
+  std::string p_plugin_list_;
+  bool p_draw_background_checkerboard_;
+  bool p_draw_free_space_grid_;
+  bool use_map_topic_;
+
+  //double p_geotiff_save_period_;
+
+  ros::NodeHandle n_;
+  ros::NodeHandle pn_;
+
+  ros::ServiceClient map_service_client_;// = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
+  ros::ServiceClient object_service_client_;
+  ros::ServiceClient path_service_client_;
+
+  ros::Subscriber sys_cmd_sub_;
+
+  std::unique_ptr<pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>> plugin_loader_;
+  std::vector<boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> > plugin_vector_;
+
+
+  GeotiffWriter geotiff_writer_;
+
+  ros::Timer map_save_timer_;
+
+  unsigned int running_saved_map_num_;
+
+  std::string start_dir_;
+};
+
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "geotiff_node");
+
+  hector_geotiff::MapGenerator mg;
+
+  //ros::NodeHandle pn_;
+  //double p_geotiff_save_period = 60.0f;
+  //pn_.param("geotiff_save_period", p_geotiff_save_period, 60.0);
+  //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, &mg, false);
+
+  ros::spin();
+
+  return 0;
+}
+

Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_saver.cpp

@@ -0,0 +1,121 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_geotiff/geotiff_writer.h"
+
+#include <cstdio>
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <nav_msgs/GetMap.h>
+
+#include <QApplication>
+
+using namespace std;
+
+namespace hector_geotiff{
+
+/**
+ * @brief Map generation node.
+ */
+class MapGenerator
+{
+  public:
+    MapGenerator(const std::string& mapname) : mapname_(mapname)
+    {
+      ros::NodeHandle n;
+      ROS_INFO("Waiting for the map");
+      map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
+    }
+
+    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
+    {
+      ros::Time start_time (ros::Time::now());
+
+      geotiff_writer.setMapFileName(mapname_);
+      geotiff_writer.setupTransforms(*map);
+      geotiff_writer.drawBackgroundCheckerboard();
+      geotiff_writer.drawMap(*map);
+      geotiff_writer.drawCoords();
+
+      geotiff_writer.writeGeotiffImage(true);
+
+      ros::Duration elapsed_time (ros::Time::now() - start_time);
+      ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec());
+    }
+
+    GeotiffWriter geotiff_writer;
+
+    std::string mapname_;
+    ros::Subscriber map_sub_;
+};
+
+}
+
+#define USAGE "Usage: \n" \
+              "  geotiff_saver -h\n"\
+              "  geotiff_saver [-f <mapname>] [ROS remapping args]"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "map_saver");
+  std::string mapname = "map";
+
+  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
+    {
+      puts(USAGE);
+      return 1;
+    }
+  }
+
+  //GeotiffWriter geotiff_writer;
+  //geotiff_writer.setMapName("test");
+  hector_geotiff::MapGenerator mg(mapname);
+
+  ros::spin();
+
+  return 0;
+}
+

Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_writer/geotiff_writer.cpp

@@ -0,0 +1,714 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_geotiff/geotiff_writer.h"
+#include <ros/console.h>
+
+#include <QFile>
+#include <QImageWriter>
+#include <QPainter>
+//#include <QtCore/QDateTime>
+#include <QTime>
+#include <QTextStream>
+#include <QFontDatabase>
+
+#include <ros/package.h>
+
+#if  __cplusplus < 201703L
+	#include <experimental/filesystem>
+	namespace fs = std::experimental::filesystem;
+#else
+	#include <filesystem>
+	namespace fs = std::filesystem;
+#endif
+
+namespace hector_geotiff
+{
+
+
+GeotiffWriter::GeotiffWriter( bool useCheckerboardCacheIn )
+  : useCheckerboardCache( useCheckerboardCacheIn )
+    , use_utc_time_suffix_( true )
+{
+  cached_map_meta_data_.height = -1;
+  cached_map_meta_data_.width = -1;
+  cached_map_meta_data_.resolution = -1.0f;
+
+  int fake_argc = 3;
+  char *fake_argv[3] = { new char[15], new char[10], new char[10] };
+  strcpy( fake_argv[0], "geotiff_writer" );
+  strcpy( fake_argv[1], "-platform" );
+  strcpy( fake_argv[2], "offscreen" ); // Set the env QT_DEBUG_PLUGINS to 1 to see available platforms
+
+  ROS_INFO("Creating application with offscreen platform.");
+  //Create a QApplication cause otherwise drawing text will crash
+  app = new QApplication( fake_argc, fake_argv );
+  delete[] fake_argv[0];
+  delete[] fake_argv[1];
+  delete[] fake_argv[2];
+  ROS_INFO("Created application");
+
+  std::string font_path = ros::package::getPath( "hector_geotiff" ) + "/fonts/Roboto-Regular.ttf";
+  int id = QFontDatabase::addApplicationFont( QString::fromStdString( font_path ));
+  font_family_ = QFontDatabase::applicationFontFamilies( id ).at( 0 );
+
+  map_file_name_ = "";
+  map_file_path_ = "";
+}
+
+GeotiffWriter::~GeotiffWriter()
+{
+  delete app;
+}
+
+void GeotiffWriter::setMapFileName( const std::string &mapFileName )
+{
+  map_file_name_ = mapFileName;
+
+  if ( use_utc_time_suffix_ )
+  {
+    //QDateTime now (QDateTime::currentDateTimeUtc());
+    //std::string current_time_string = now.toString(Qt::ISODate).toStdString();
+    QTime now( QTime::currentTime());
+    std::string current_time_string = now.toString( Qt::ISODate ).toStdString();
+
+    map_file_name_ += "_" + current_time_string;
+  }
+}
+
+void GeotiffWriter::setMapFilePath( const std::string &mapFilePath )
+{
+  map_file_path_ = mapFilePath;
+}
+
+void GeotiffWriter::setUseUtcTimeSuffix( bool useSuffix )
+{
+  use_utc_time_suffix_ = useSuffix;
+}
+
+
+bool GeotiffWriter::setupTransforms( const nav_msgs::OccupancyGrid &map )
+{
+  resolution = static_cast<float>(map.info.resolution);
+  origin = Eigen::Vector2f( map.info.origin.position.x, map.info.origin.position.y );
+
+  resolutionFactor = 3;
+  resolutionFactorf = static_cast<float>(resolutionFactor);
+
+  pixelsPerMapMeter = 1.0f / map.info.resolution;
+  pixelsPerGeoTiffMeter = pixelsPerMapMeter * static_cast<float>(resolutionFactor);
+
+  minCoordsMap = Eigen::Vector2i::Zero();
+  maxCoordsMap = Eigen::Vector2i( map.info.width, map.info.height );
+
+  if ( !HectorMapTools::getMapExtends( map, minCoordsMap, maxCoordsMap ))
+  {
+    ROS_INFO( "Cannot determine map extends!" );
+    return false;
+  }
+
+  sizeMap = Eigen::Vector2i( maxCoordsMap - minCoordsMap );
+  sizeMapf = ((maxCoordsMap - minCoordsMap).cast<float>());
+
+
+  rightBottomMarginMeters = Eigen::Vector2f( 1.0f, 1.0f );
+  rightBottomMarginPixelsf = Eigen::Vector2f( rightBottomMarginMeters.array() * pixelsPerGeoTiffMeter );
+  rightBottomMarginPixels = ((rightBottomMarginPixelsf.array() + 0.5f).cast<int>());
+
+  leftTopMarginMeters = Eigen::Vector2f( 3.0f, 3.0f );
+
+  totalMeters = (rightBottomMarginMeters + sizeMapf * map.info.resolution + leftTopMarginMeters);
+  //std::cout << "\n" << totalMeters;
+
+  totalMeters.x() = ceil( totalMeters.x());
+  totalMeters.y() = ceil( totalMeters.y());
+  //std::cout << "\n" << totalMeters;
+
+  geoTiffSizePixels = ((totalMeters.array() * pixelsPerGeoTiffMeter).cast<int>());
+
+
+  mapOrigInGeotiff = (rightBottomMarginPixelsf);
+  mapEndInGeotiff = (rightBottomMarginPixelsf + sizeMapf * resolutionFactorf);
+  //std::cout << "\n mapOrig\n" << mapOrigInGeotiff;
+  //std::cout << "\n mapOrig\n" << mapEndInGeotiff;
+
+  world_map_transformer_.setTransforms( map );
+
+  map_geo_transformer_.setTransformsBetweenCoordSystems( mapOrigInGeotiff, mapEndInGeotiff, minCoordsMap.cast<float>(),
+                                                         maxCoordsMap.cast<float>());
+
+  /*
+  Eigen::Vector2f temp_zero_map_g (map_geo_transformer_.getC2Coords(Eigen::Vector2f::Zero()));
+
+  Eigen::Vector2f temp_zero_map_g_floor (floor(temp_zero_map_g.x()), floor(temp_zero_map_g.x()));
+
+  Eigen::Vector2f diff (temp_zero_map_g - temp_zero_map_g_floor);
+
+  map*/
+
+
+  Eigen::Vector2f p1_w( Eigen::Vector2f::Zero());
+  Eigen::Vector2f p2_w( Eigen::Vector2f( 100.0f, 100.0f ));
+
+  Eigen::Vector2f p1_m( world_map_transformer_.getC2Coords( p1_w ));
+  Eigen::Vector2f p2_m( world_map_transformer_.getC2Coords( p2_w ));
+
+  Eigen::Vector2f p1_g( map_geo_transformer_.getC2Coords( p1_m ));
+  Eigen::Vector2f p2_g( map_geo_transformer_.getC2Coords( p2_m ));
+
+  world_geo_transformer_.setTransformsBetweenCoordSystems( p1_g, p2_g, p1_w, p2_w );
+
+  map_draw_font_ = QFont( font_family_ );
+  map_draw_font_.setPixelSize( 6 * resolutionFactor );
+
+  if ( useCheckerboardCache )
+  {
+
+    if ((cached_map_meta_data_.height != map.info.height) ||
+        (cached_map_meta_data_.width != map.info.width) ||
+        (cached_map_meta_data_.resolution = map.info.resolution))
+    {
+
+      cached_map_meta_data_ = map.info;
+
+      Eigen::Vector2f img_size( Eigen::Vector2f( map.info.width, map.info.height ) * resolutionFactorf +
+                                (rightBottomMarginMeters + leftTopMarginMeters) * pixelsPerGeoTiffMeter );
+      checkerboard_cache = QImage( img_size.y(), img_size.x(), QImage::Format_RGB32 );
+
+      QPainter qPainter( &image );
+      transformPainterToImgCoords( qPainter );
+
+      QBrush c1 = QBrush( QColor( 226, 226, 227 ));
+      QBrush c2 = QBrush( QColor( 237, 237, 238 ));
+      QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter );
+
+
+      int xMaxGeo = geoTiffSizePixels[0];
+      int yMaxGeo = geoTiffSizePixels[1];
+
+      for ( int y = 0; y < yMaxGeo; ++y )
+      {
+        for ( int x = 0; x < xMaxGeo; ++x )
+        {
+          //std::cout << "\n" << x << " " << y;
+
+          if ((x + y) % 2 == 0 )
+          {
+            //qPainter.fillRect(background_grid_tile, c1);
+            qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
+                               static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
+                               pixelsPerGeoTiffMeter, c1 );
+          }
+          else
+          {
+            //qPainter.fillRect(background_grid_tile, c2);
+            qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
+                               static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
+                               pixelsPerGeoTiffMeter, c2 );
+          }
+          //background_grid_tile.moveTo(QPointF(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter));
+        }
+      }
+    }
+  }
+
+  return true;
+}
+
+void GeotiffWriter::setupImageSize()
+{
+  bool painter_rotate = true;
+
+  int xMaxGeo = geoTiffSizePixels[0];
+  int yMaxGeo = geoTiffSizePixels[1];
+
+  if ( !useCheckerboardCache )
+  {
+    if ( painter_rotate )
+    {
+      image = QImage( yMaxGeo, xMaxGeo, QImage::Format_RGB32 );
+    }
+    else
+    {
+      image = QImage( xMaxGeo, yMaxGeo, QImage::Format_RGB32 );
+    }
+
+    QPainter qPainter( &image );
+
+    QBrush grey = QBrush( QColor( 128, 128, 128 ));
+
+    qPainter.fillRect( image.rect(), grey );
+  }
+}
+
+void GeotiffWriter::drawBackgroundCheckerboard()
+{
+  int xMaxGeo = geoTiffSizePixels[0];
+  int yMaxGeo = geoTiffSizePixels[1];
+
+  bool painter_rotate = true;
+
+  if ( !useCheckerboardCache )
+  {
+
+    QPainter qPainter( &image );
+
+    if ( painter_rotate )
+    {
+      transformPainterToImgCoords( qPainter );
+    }
+
+    //*********************** Background checkerboard pattern **********************
+    QBrush c1 = QBrush( QColor( 226, 226, 227 ));
+    QBrush c2 = QBrush( QColor( 237, 237, 238 ));
+    QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter );
+
+
+    for ( int y = 0; y < yMaxGeo; ++y )
+    {
+      for ( int x = 0; x < xMaxGeo; ++x )
+      {
+        //std::cout << "\n" << x << " " << y;
+
+        if ((x + y) % 2 == 0 )
+        {
+          //qPainter.fillRect(background_grid_tile, c1);
+          qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
+                             static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
+                             pixelsPerGeoTiffMeter, c1 );
+        }
+        else
+        {
+          //qPainter.fillRect(background_grid_tile, c2);
+          qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
+                             static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
+                             pixelsPerGeoTiffMeter, c2 );
+        }
+        //background_grid_tile.moveTo(QPointF(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter));
+      }
+    }
+  }
+  else
+  {
+    image = checkerboard_cache.copy( 0, 0, geoTiffSizePixels[0], geoTiffSizePixels[1] );
+  }
+}
+
+void GeotiffWriter::drawMap( const nav_msgs::OccupancyGrid &map, bool draw_explored_space_grid )
+{
+  QPainter qPainter( &image );
+
+  transformPainterToImgCoords( qPainter );
+
+  //this->drawCoordSystem(qPainter);
+
+  QRectF map_cell_grid_tile( 0.0f, 0.0f, resolutionFactor, resolutionFactor );
+
+  QBrush occupied_brush( QColor( 0, 40, 120 ));
+  QBrush free_brush( QColor( 255, 255, 255 ));
+  QBrush explored_space_grid_brush( QColor( 190, 190, 191 ));
+
+  int width = map.info.width;
+
+  float explored_space_grid_resolution_pixels = pixelsPerGeoTiffMeter * 0.5f;
+
+  float yGeo = 0.0f;
+  float currYLimit = 0.0f;
+
+  bool drawY = false;
+
+  for ( int y = minCoordsMap[1]; y < maxCoordsMap[1]; ++y )
+  {
+
+    float xGeo = 0.0f;
+
+    if ( yGeo >= currYLimit )
+    {
+      drawY = true;
+    }
+
+    float currXLimit = 0.0f;
+    bool drawX = false;
+
+    for ( int x = minCoordsMap[0]; x < maxCoordsMap[0]; ++x )
+    {
+
+      unsigned int i = y * width + x;
+
+      int8_t data = map.data[i];
+
+      if ( xGeo >= currXLimit )
+      {
+        drawX = true;
+      }
+
+      if ( data == 0 )
+      {
+
+        Eigen::Vector2f coords( mapOrigInGeotiff + (Eigen::Vector2f( xGeo, yGeo )));
+        qPainter.fillRect( coords[0], coords[1], resolutionFactorf, resolutionFactorf, free_brush );
+
+
+        if ( draw_explored_space_grid )
+        {
+          if ( drawY )
+          {
+            qPainter.fillRect( coords[0], mapOrigInGeotiff.y() + currYLimit, resolutionFactorf, 1.0f,
+                               explored_space_grid_brush );
+          }
+
+          if ( drawX )
+          {
+            qPainter.fillRect( mapOrigInGeotiff.x() + currXLimit, coords[1], 1.0f, resolutionFactorf,
+                               explored_space_grid_brush );
+          }
+        }
+      }
+      else if ( data == 100 )
+      {
+        qPainter.fillRect( mapOrigInGeotiff.x() + xGeo, mapOrigInGeotiff.y() + yGeo, resolutionFactorf,
+                           resolutionFactorf, occupied_brush );
+      }
+
+      if ( drawX )
+      {
+        currXLimit += explored_space_grid_resolution_pixels;
+        drawX = false;
+      }
+
+      xGeo += resolutionFactorf;
+    }
+
+    if ( drawY )
+    {
+      drawY = false;
+      currYLimit += explored_space_grid_resolution_pixels;
+    }
+
+    yGeo += resolutionFactorf;
+  }
+}
+
+void GeotiffWriter::drawObjectOfInterest( const Eigen::Vector2f &coords, const std::string &txt, const Color &color,
+                                          const Shape &shape )
+{
+  QPainter qPainter( &image );
+
+  transformPainterToImgCoords( qPainter );
+
+
+  //qPainter.setPen(ellipse_pen_);
+
+  Eigen::Vector2f map_coords( world_map_transformer_.getC2Coords( coords ));
+
+  Eigen::Vector2f coords_g( world_geo_transformer_.getC2Coords( coords ));
+
+  qPainter.translate( coords_g[0], coords_g[1] );
+
+  qPainter.rotate( 90 );
+
+  qPainter.setRenderHint( QPainter::Antialiasing, true );
+
+  float radius = pixelsPerGeoTiffMeter * 0.175f;
+
+  QRectF shape_rect( -radius, -radius, radius * 2.0f, radius * 2.0f );
+  qPainter.save();
+
+  QBrush tmpBrush( QColor( color.r, color.g, color.b ));
+  QPen tmpPen( Qt::NoPen );
+  qPainter.setBrush( tmpBrush );
+  qPainter.setPen( tmpPen );
+
+  if ( shape == SHAPE_CIRCLE )
+  {
+    qPainter.drawEllipse( shape_rect );
+  }
+  else if ( shape == SHAPE_DIAMOND )
+  {
+    qPainter.rotate( 45 );
+    qPainter.drawRect( shape_rect );
+  }
+
+  qPainter.restore();
+
+
+  QString tmp( txt.c_str());
+  //tmp.setNum(number);
+
+  if ( tmp.length() < 2 )
+  {
+    qPainter.setFont( map_draw_font_ );
+  }
+  else
+  {
+    QFont tmp_font( font_family_ );
+    tmp_font.setPixelSize( 3 * resolutionFactor );
+    qPainter.setFont( tmp_font );
+  }
+
+
+  qPainter.setPen( Qt::white );
+  qPainter.scale( -1.0, 1.0 );
+
+  qPainter.drawText( shape_rect, Qt::AlignCenter, tmp );
+}
+
+void GeotiffWriter::drawPath( const Eigen::Vector3f &start, const std::vector<Eigen::Vector2f> &points, int color_r,
+                              int color_g, int color_b )
+{
+  QPainter qPainter( &image );
+
+  transformPainterToImgCoords( qPainter );
+
+  Eigen::Vector2f start_geo( world_geo_transformer_.getC2Coords( start.head<2>()));
+
+
+  size_t size = points.size();
+
+  QPolygonF polygon;
+  polygon.reserve( size );
+
+  polygon.push_back( QPointF( start_geo.x(), start_geo.y()));
+
+  for ( size_t i = 0; i < size; ++i )
+  {
+    const Eigen::Vector2f vec( world_geo_transformer_.getC2Coords( points[i] ));
+    polygon.push_back( QPointF( vec.x(), vec.y()));
+  }
+
+  QPen pen( qPainter.pen());
+  pen.setColor( QColor( color_r, color_g, color_b ));
+  pen.setWidth( 3 );
+
+  qPainter.setPen( pen );
+
+  //qPainter.setPen(QColor(120,0,240));
+
+
+  qPainter.drawPolyline( polygon );
+
+  qPainter.save();
+  qPainter.translate( start_geo.x(), start_geo.y());
+  qPainter.rotate( start.z());
+  qPainter.setRenderHint( QPainter::Antialiasing, true );
+  drawArrow( qPainter );
+  //drawCoordSystem(qPainter);
+  qPainter.restore();
+}
+
+std::string GeotiffWriter::getBasePathAndFileName() const
+{
+  return std::string( map_file_path_ + "/" + map_file_name_ );
+}
+
+void GeotiffWriter::writeGeotiffImage(bool completed)
+{
+  //Only works with recent Qt versions
+  //QDateTime now (QDateTime::currentDateTimeUtc());
+  //std::string current_time_string = now.toString(Qt::ISODate).toStdString();
+  std::string complete_file_string;
+  if (completed) {
+    complete_file_string = map_file_path_ + "/" + map_file_name_;
+  }
+  else {
+    auto t = std::time(nullptr);
+    auto tm = *std::localtime(&t);
+    std::stringstream start_ss;
+    start_ss <<  std::put_time(&tm, "%Y-%m-%d");
+    
+    complete_file_string = map_file_path_ + "/autosave";
+    std::error_code error;
+    if(!fs::exists(complete_file_string.c_str())) {
+      fs::create_directory(complete_file_string.c_str(), error);
+      if (error) {
+        ROS_ERROR("Can't create autosave folder");
+        return;
+      }
+    }
+
+    complete_file_string += "/" + start_ss.str();
+    if(!fs::exists(complete_file_string.c_str())) {
+      fs::create_directory(complete_file_string.c_str(), error);
+      if (error) {
+        ROS_ERROR("Can't create folder in autosave");
+        return;
+      }
+    }
+
+    complete_file_string += "/" + map_file_name_;
+  }
+  
+  QImageWriter imageWriter( QString::fromStdString( complete_file_string + ".tif" ));
+  imageWriter.setCompression( 1 );
+
+  bool success = imageWriter.write( image );
+
+  std::string tfw_file_name( complete_file_string + ".tfw" );
+  QFile tfwFile( QString::fromStdString( tfw_file_name ));
+
+  tfwFile.open( QIODevice::WriteOnly );
+
+  QTextStream out( &tfwFile );
+
+  float resolution_geo = resolution / resolutionFactorf;
+
+  QString resolution_string;
+  resolution_string.setNum( resolution_geo, 'f', 10 );
+
+  //positive x resolution
+  out << resolution_string << "\n";
+
+  QString zero_string;
+  zero_string.setNum( 0.0f, 'f', 10 );
+
+  //rotation, translation
+  out << zero_string << "\n" << zero_string << "\n";
+
+  //negative y resolution
+  out << "-" << resolution_string << "\n";
+
+  QString top_left_string_x;
+  QString top_left_string_y;
+
+  //Eigen::Vector2f zero_map_w = world_map_transformer_.getC1Coords(Eigen::Vector2f::Zero());
+  Eigen::Vector2f zero_geo_w( world_geo_transformer_.getC1Coords((geoTiffSizePixels.array() + 1).cast<float>()));
+
+
+  top_left_string_x.setNum( -zero_geo_w.y(), 'f', 10 );
+  top_left_string_y.setNum( zero_geo_w.x(), 'f', 10 );
+
+  out << top_left_string_x << "\n" << top_left_string_y << "\n";
+
+  tfwFile.close();
+
+  if ( !success )
+  {
+    ROS_INFO( "Writing image with file %s failed with error %s", complete_file_string.c_str(),
+              imageWriter.errorString().toStdString().c_str());
+  }
+  else
+  {
+    ROS_INFO( "Successfully wrote geotiff to %s", complete_file_string.c_str());
+  }
+}
+
+void GeotiffWriter::transformPainterToImgCoords( QPainter &painter )
+{
+  painter.rotate( -90 );
+  painter.translate( -geoTiffSizePixels.x(), geoTiffSizePixels.y());
+  painter.scale( 1.0, -1.0 );
+}
+
+void GeotiffWriter::drawCoords()
+{
+  QPainter qPainter( &image );
+  qPainter.setFont( map_draw_font_ );
+
+  float arrowOffset = pixelsPerGeoTiffMeter * 0.15f;
+
+  // MAP ORIENTATION
+  qPainter.setPen( QColor( 0, 50, 140 ));
+  qPainter.drawLine( pixelsPerGeoTiffMeter / 2, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter / 2,
+                     2.0f * pixelsPerGeoTiffMeter );
+  qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, pixelsPerGeoTiffMeter - 1, pixelsPerGeoTiffMeter * 3 / 5,
+                     pixelsPerGeoTiffMeter - 1 );
+  qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter * 3 / 5,
+                     2 * pixelsPerGeoTiffMeter );
+
+
+  qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter,
+                     2 * pixelsPerGeoTiffMeter );
+  qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset,
+                     2 * pixelsPerGeoTiffMeter - arrowOffset );
+  qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset,
+                     2 * pixelsPerGeoTiffMeter + arrowOffset );
+
+  qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter,
+                     2 * pixelsPerGeoTiffMeter );
+  qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter + arrowOffset,
+                     pixelsPerGeoTiffMeter + arrowOffset );
+  qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter - arrowOffset,
+                     pixelsPerGeoTiffMeter + arrowOffset );
+
+  qPainter.drawText( 0.6 * pixelsPerGeoTiffMeter, 1.6 * pixelsPerGeoTiffMeter, QString( "1m" ));
+
+  qPainter.drawText( 2.2 * pixelsPerGeoTiffMeter, 1.1 * pixelsPerGeoTiffMeter, QString( "x" ));
+  qPainter.drawText( 1.2 * pixelsPerGeoTiffMeter, 1.8 * pixelsPerGeoTiffMeter, QString( "y" ));
+
+  qPainter.drawText( 0.5f * pixelsPerGeoTiffMeter, 0.75f * pixelsPerGeoTiffMeter,
+                     QString((map_file_name_ + ".tif").c_str()));
+}
+
+void GeotiffWriter::drawCross( QPainter &painter, const Eigen::Vector2f &coords )
+{
+  painter.drawLine( QPointF( coords[0] - 1.0f, coords[1] ), QPointF( coords[0] + 1.0f, coords[1] ));
+  painter.drawLine( QPointF( coords[0], coords[1] - 1.0f ), QPointF( coords[0], coords[1] + 1.0f ));
+}
+
+void GeotiffWriter::drawArrow( QPainter &painter )
+{
+  float tip_distance = pixelsPerGeoTiffMeter * 0.3f;
+
+  QPolygonF polygon;
+
+  polygon << QPointF( tip_distance, 0.0f ) << QPointF( -tip_distance * 0.5f, -tip_distance * 0.5f )
+          << QPointF( 0.0f, 0.0f ) << QPointF( -tip_distance * 0.5f, tip_distance * 0.5f );
+
+  painter.save();
+
+  QBrush tmpBrush( QColor( 255, 200, 0 ));
+  QPen tmpPen( Qt::NoPen );
+  painter.setBrush( tmpBrush );
+  painter.setPen( tmpPen );
+
+  painter.drawPolygon( polygon );
+
+  painter.restore();
+}
+
+void GeotiffWriter::drawCoordSystem( QPainter &painter )
+{
+  painter.save();
+  QPointF zero_point( 0.0f, 0.0f );
+  QPointF x_point( pixelsPerGeoTiffMeter, 0.0f );
+  QPointF y_point( 0.0f, pixelsPerGeoTiffMeter );
+
+  QPen tmp = painter.pen();
+  tmp.setWidth( 5 );
+  tmp.setColor( QColor( 255.0, 0.0, 0.0 ));
+  //painter.setPen(QPen::setWidth(5));
+  painter.setPen( tmp );
+  painter.drawLine( zero_point, x_point );
+
+  tmp.setColor( QColor( 0, 255, 0 ));
+  painter.setPen( tmp );
+  painter.drawLine( zero_point, y_point );
+
+  painter.restore();
+}
+}

Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CHANGELOG.rst

@@ -0,0 +1,15 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_geotiff_launch
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Contributors: Stefan Fabian

Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_geotiff_launch)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="trajectory_source_frame_name" default="/base_link"/>
+  <arg name="trajectory_update_rate" default="4"/>
+  <arg name="trajectory_publish_rate" default="0.25"/>
+  <arg name="map_file_path" default="$(find hector_geotiff)/maps"/>
+  <arg name="map_file_base_name" default="hector_slam_map"/>
+
+  <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
+    <param name="target_frame_name" type="string" value="/map" />
+    <param name="source_frame_name" type="string" value="$(arg trajectory_source_frame_name)" />
+    <param name="trajectory_update_rate" type="double" value="$(arg trajectory_update_rate)" />
+    <param name="trajectory_publish_rate" type="double" value="$(arg trajectory_publish_rate)" />
+  </node>
+
+  <node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
+    <remap from="map" to="/dynamic_map" />
+    <param name="map_file_path" type="string" value="$(arg map_file_path)" />
+    <param name="map_file_base_name" type="string" value="$(arg map_file_base_name)" />
+    <param name="geotiff_save_period" type="double" value="0" />
+    <param name="draw_background_checkerboard" type="bool" value="true" />
+    <param name="draw_free_space_grid" type="bool" value="true" />
+    <param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter" />
+  </node>
+
+</launch>
+

Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper_only.launch

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
+    <remap from="map" to="scanmatcher_map" />
+    <param name="map_file_path" type="string" value="$(find hector_geotiff)/maps" />
+    <param name="map_file_base_name" type="string" value="" />
+    <param name="geotiff_save_period" type="double" value="45" />
+
+    <param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter hector_worldmodel_geotiff_plugins/QRCodeMapWriter hector_worldmodel_geotiff_plugins/VictimMapWriter" />
+    <param name="VictimMapWriter/draw_all_objects" value="false" />
+    <param name="VictimMapWriter/class_id" value="victim" />
+    <param name="QRCodeMapWriter/draw_all_objects" value="true" />
+    <param name="QRCodeMapWriter/class_id" value="qrcode" />
+  </node>
+
+</launch>
+

Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hector_geotiff_launch</name>
+  <version>0.5.2</version>
+  <description>Contains launch files for the hector_geotiff mapper.</description>
+
+  <maintainer email="fabian@sim.tu-darmstadt.de">Stefan Fabian</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_geotiff</url>
+
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>hector_geotiff</exec_depend>
+  <exec_depend>hector_geotiff_plugins</exec_depend>
+  <exec_depend>hector_trajectory_server</exec_depend>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CHANGELOG.rst

@@ -0,0 +1,51 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_geotiff_plugins
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node.
+  Some minor cleanup.
+* Contributors: Stefan Fabian
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths
+* Contributors: Dorothea Koert
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* readded PLUGINLIB_DECLARE_CLASS macro for fuerte compatibility
+* use hector_geotiff_plugins/TrajectoryMapWriter as legacy lookup name for the trajectory geotiff plugin to not break old launch files
+* fixed warnings for deprecated pluginlib method/macro calls
+* added changelogs
+* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_geotiff_plugins)
+
+find_package(catkin REQUIRED COMPONENTS hector_geotiff hector_nav_msgs)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS hector_geotiff hector_nav_msgs
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(hector_geotiff_plugins src/trajectory_geotiff_plugin.cpp)
+add_dependencies(hector_geotiff_plugins ${catkin_EXPORTED_TARGETS})
+target_link_libraries(hector_geotiff_plugins
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS hector_geotiff_plugins
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES
+  hector_geotiff_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libhector_geotiff_plugins">
+  <class name="hector_geotiff_plugins/TrajectoryMapWriter" type="hector_geotiff_plugins::TrajectoryMapWriter" base_class_type="hector_geotiff::MapWriterPluginInterface">
+    <description>
+      This is a MapWriter plugin that draws the robot's trajectory in the map.
+    </description>
+  </class>
+</library>

Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_geotiff_plugins</name>
+  <version>0.5.2</version>
+  <description>
+    hector_geotiff_plugins contains plugins that extend geotiff maps generated by hector_geotiff.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_geotiff_plugins</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+  <author>Gregor Gebhardt</author>
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>hector_geotiff</build_depend>
+  <build_depend>hector_nav_msgs</build_depend>
+  <run_depend>hector_geotiff</run_depend>
+  <run_depend>hector_nav_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+    <hector_geotiff plugin="${prefix}/hector_geotiff_plugins.xml" />
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp

@@ -0,0 +1,123 @@
+//=================================================================================================
+// Copyright (c) 2012, Gregor Gebhardt, TU Darmstadt
+// 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 Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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 <hector_geotiff/map_writer_interface.h>
+#include <hector_geotiff/map_writer_plugin_interface.h>
+
+#include <ros/ros.h>
+#include <hector_nav_msgs/GetRobotTrajectory.h>
+
+#include <fstream>
+
+namespace hector_geotiff_plugins
+{
+
+using namespace hector_geotiff;
+
+class TrajectoryMapWriter : public MapWriterPluginInterface
+{
+public:
+  TrajectoryMapWriter();
+  virtual ~TrajectoryMapWriter();
+
+  virtual void initialize(const std::string& name);
+  virtual void draw(MapWriterInterface *interface);
+
+protected:
+  ros::NodeHandle nh_;
+  ros::ServiceClient service_client_;
+
+  bool initialized_;
+  std::string name_;
+  bool draw_all_objects_;
+  std::string class_id_;
+  int path_color_r_;
+  int path_color_g_;
+  int path_color_b_;
+};
+
+TrajectoryMapWriter::TrajectoryMapWriter()
+    : initialized_(false)
+{}
+
+TrajectoryMapWriter::~TrajectoryMapWriter()
+{}
+
+void TrajectoryMapWriter::initialize(const std::string& name)
+{
+  ros::NodeHandle plugin_nh("~/" + name);
+  std::string service_name_;
+
+
+  plugin_nh.param("service_name", service_name_, std::string("trajectory"));
+  plugin_nh.param("path_color_r", path_color_r_, 120);
+  plugin_nh.param("path_color_g", path_color_g_, 0);
+  plugin_nh.param("path_color_b", path_color_b_, 240);
+
+  service_client_ = nh_.serviceClient<hector_nav_msgs::GetRobotTrajectory>(service_name_);
+
+  initialized_ = true;
+  this->name_ = name;
+  ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
+}
+
+void TrajectoryMapWriter::draw(MapWriterInterface *interface)
+{
+    if(!initialized_) return;
+
+    hector_nav_msgs::GetRobotTrajectory srv_path;
+    if (!service_client_.call(srv_path)) {
+      ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str());
+      return;
+    }
+
+    std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
+
+    size_t size = traj_vector.size();
+
+    std::vector<Eigen::Vector2f> pointVec;
+    pointVec.resize(size);
+
+    for (size_t i = 0; i < size; ++i){
+      const geometry_msgs::PoseStamped& pose (traj_vector[i]);
+
+      pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
+    }
+
+    if (size > 0){
+      //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
+      Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
+      interface->drawPath(startVec, pointVec, path_color_r_, path_color_g_, path_color_b_);
+    }
+}
+
+} // namespace
+
+//register this planner as a MapWriterPluginInterface plugin
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)

Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst

@@ -0,0 +1,45 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_imu_attitude_to_tf
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+* hector_imu_attitude_to_tf: fixed default values of the base_frame and base_stabilized_frame parameters (fix #20)
+* Contributors: Johannes Meyer
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added missing install rule for launch files
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_imu_attitude_to_tf)
+
+find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs tf)
+
+catkin_package(
+  CATKIN_DEPENDS roscpp sensor_msgs tf
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(imu_attitude_to_tf_node src/imu_attitude_to_tf_node.cpp)
+target_link_libraries(imu_attitude_to_tf_node ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS imu_attitude_to_tf_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/launch/example.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="hector_imu_attitude_to_tf" type="imu_attitude_to_tf_node" name="imu_attitude_to_tf_node" output="screen">
+    <remap from="imu_topic" to="thumper_imu" />
+    <param name="base_stabilized_frame" type="string" value="base_stabilized" />
+    <param name="base_frame" type="string" value="base_footprint" />
+  </node>
+</launch>

Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/package.xml

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hector_imu_attitude_to_tf</name>
+  <version>0.5.2</version>
+  <description>
+    hector_imu_attitude_to_tf is a lightweight node that can be used to publish the roll/pitch attitude angles reported via a imu message to tf.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://ros.org/wiki/hector_imu_attitude_to_tf</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp

@@ -0,0 +1,84 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "tf/transform_broadcaster.h"
+#include "sensor_msgs/Imu.h"
+
+std::string p_base_stabilized_frame_;
+std::string p_base_frame_;
+tf::TransformBroadcaster* tfB_;
+tf::StampedTransform transform_;
+tf::Quaternion tmp_;
+
+#ifndef TF_MATRIX3x3_H
+  typedef btScalar tfScalar;
+  namespace tf { typedef btMatrix3x3 Matrix3x3; }
+#endif
+
+void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
+{
+  tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
+
+  tfScalar yaw, pitch, roll;
+  tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
+
+  tmp_.setRPY(roll, pitch, 0.0);
+
+  transform_.setRotation(tmp_);
+
+  transform_.stamp_ = imu_msg.header.stamp;
+
+  tfB_->sendTransform(transform_);
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, ROS_PACKAGE_NAME);
+
+  ros::NodeHandle n;
+  ros::NodeHandle pn("~");
+
+  pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
+  pn.param("base_frame", p_base_frame_, std::string("base_link"));
+  
+  tfB_ = new tf::TransformBroadcaster();
+  transform_.getOrigin().setX(0.0);
+  transform_.getOrigin().setY(0.0);
+  transform_.getOrigin().setZ(0.0);
+  transform_.frame_id_ = p_base_stabilized_frame_;
+  transform_.child_frame_id_ = p_base_frame_;
+
+  ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback);
+
+  ros::spin();
+
+  delete tfB_;
+
+  return 0;
+}

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CHANGELOG.rst

@@ -0,0 +1,41 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_imu_tools
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+* Fix sim setup
+* remap for bertl setup
+* Contributors: Florian Kunz, kohlbrecher
+
+0.3.3 (2014-06-15)
+------------------
+* hector_imu_tools: Basics of simple height etimation
+* hector_imu_tools: Add tf publishers in hector_imu_tools
+* hector_imu_tools: Also write out fake odometry
+* hector_imu_tools: Fix typo
+* hector_imu_tools: Prevent race conditions in slam, formatting
+* hector_imu_tools: Small executable for generating a IMU message out of a (2d) pose and roll/pitch imu message
+* Contributors: Stefan Kohlbrecher

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_imu_tools)
+
+find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs nav_msgs tf)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp sensor_msgs tf
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(pose_and_orientation_to_imu_node src/pose_and_orientation_to_imu_node.cpp)
+
+target_link_libraries(pose_and_orientation_to_imu_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS pose_and_orientation_to_imu_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
+)

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/bertl_robot.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+
+<launch>
+  <node pkg="hector_imu_tools" type="pose_and_orientation_to_imu_node" name="pose_and_orientation_to_imu_node" output="screen">
+    <remap from="/imu" to="/imu_quat" />
+    <remap from="/fused_imu" to="/imu_in" />
+    <remap from="/pose" to="/slam_out_pose" />
+    <!--<remap from="/state" to="/state_imu" />-->
+  </node>
+</launch>

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/jasmine_robot.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<launch>
+  <node pkg="hector_imu_tools" type="pose_and_orientation_to_imu_node" name="pose_and_orientation_to_imu_node" output="screen">
+    <remap from="/imu" to="/imu_quat" />
+    <remap from="/fused_imu" to="/imu_in" />
+    <remap from="/pose" to="/slam_out_pose" />
+  </node>
+</launch>

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_imu_tools</name>
+  <version>0.5.2</version>
+  <description>
+    hector_imu_tools provides some tools for processing IMU messages
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://ros.org/wiki/hector_imu_attitude_to_tf</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp

@@ -0,0 +1,190 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "tf/transform_broadcaster.h"
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+
+std::string p_map_frame_;
+std::string p_base_footprint_frame_;
+std::string p_base_stabilized_frame_;
+std::string p_base_frame_;
+tf::TransformBroadcaster* tfB_;
+tf::StampedTransform transform_;
+
+tf::Quaternion robot_pose_quaternion_;
+tf::Point robot_pose_position_;
+tf::Transform robot_pose_transform_;
+
+tf::Quaternion tmp_;
+tf::Quaternion orientation_quaternion_;
+
+sensor_msgs::ImuConstPtr last_imu_msg_;
+sensor_msgs::Imu fused_imu_msg_;
+nav_msgs::Odometry odom_msg_;
+geometry_msgs::PoseStampedConstPtr last_pose_msg_;
+
+ros::Publisher fused_imu_publisher_;
+ros::Publisher odometry_publisher_;
+
+size_t callback_count_;
+
+#ifndef TF_MATRIX3x3_H
+typedef btScalar tfScalar;
+namespace tf { typedef btMatrix3x3 Matrix3x3; }
+#endif
+
+void imuMsgCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
+{
+  callback_count_++;
+
+  tf::quaternionMsgToTF(imu_msg->orientation, tmp_);
+
+  tfScalar imu_yaw, imu_pitch, imu_roll;
+  tf::Matrix3x3(tmp_).getRPY(imu_roll, imu_pitch, imu_yaw);
+
+  tf::Transform transform;
+  transform.setIdentity();
+  tf::Quaternion quat;
+
+  quat.setRPY(imu_roll, imu_pitch, 0.0);
+
+  if (true){
+    transform.setRotation(quat);
+    tfB_->sendTransform(tf::StampedTransform(transform, imu_msg->header.stamp, p_base_stabilized_frame_, p_base_frame_));
+  }
+
+  tfScalar pose_yaw, pose_pitch, pose_roll;
+
+  if (last_pose_msg_ != 0){
+    tf::quaternionMsgToTF(last_pose_msg_->pose.orientation, tmp_);
+
+    tf::Matrix3x3(tmp_).getRPY(pose_roll, pose_pitch, pose_yaw);
+  }else{
+    pose_yaw = 0.0;
+  }
+
+  orientation_quaternion_.setRPY(imu_roll, imu_pitch, pose_yaw);
+
+  fused_imu_msg_.header.stamp = imu_msg->header.stamp;
+
+  fused_imu_msg_.orientation.x = orientation_quaternion_.getX();
+  fused_imu_msg_.orientation.y = orientation_quaternion_.getY();
+  fused_imu_msg_.orientation.z = orientation_quaternion_.getZ();
+  fused_imu_msg_.orientation.w = orientation_quaternion_.getW();
+
+  fused_imu_publisher_.publish(fused_imu_msg_);
+
+  //If no pose message received, yaw is set to 0.
+  //@TODO: Check for timestamp of pose and disable sending if too old
+  if (last_pose_msg_ != 0){
+    if ( (callback_count_ % 5) == 0){
+      odom_msg_.header.stamp = imu_msg->header.stamp;
+      odom_msg_.pose.pose.orientation = fused_imu_msg_.orientation;
+      odom_msg_.pose.pose.position = last_pose_msg_->pose.position;
+
+      odometry_publisher_.publish(odom_msg_);
+    }
+  }
+
+}
+
+void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
+{
+
+  std::vector<tf::StampedTransform> transforms;
+  transforms.resize(2);
+
+  tf::quaternionMsgToTF(pose_msg->pose.orientation, robot_pose_quaternion_);
+  tf::pointMsgToTF(pose_msg->pose.position, robot_pose_position_);
+
+  robot_pose_transform_.setRotation(robot_pose_quaternion_);
+  robot_pose_transform_.setOrigin(robot_pose_position_);
+
+  tf::Transform height_transform;
+  height_transform.setIdentity();
+  height_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
+
+  transforms[0] = tf::StampedTransform(robot_pose_transform_, pose_msg->header.stamp, p_map_frame_, p_base_footprint_frame_);
+  transforms[1] = tf::StampedTransform(height_transform, pose_msg->header.stamp, p_base_footprint_frame_, p_base_stabilized_frame_);
+
+  tfB_->sendTransform(transforms);
+
+  // Perform simple estimation of vehicle altitude based on orientation
+  if (last_pose_msg_){
+    tf::Vector3 plane_normal = tf::Matrix3x3(orientation_quaternion_) * tf::Vector3(0.0, 0.0, 1.0);
+
+    tf::Vector3 last_position;
+    tf::pointMsgToTF(last_pose_msg_->pose.position, last_position);
+
+    double height_difference =
+        (-plane_normal.getX() * (robot_pose_position_.getX() - last_position.getX())
+         -plane_normal.getY() * (robot_pose_position_.getY() - last_position.getY())
+         +plane_normal.getZ() * last_position.getZ()) / last_position.getZ();
+
+  }
+
+
+
+  last_pose_msg_ = pose_msg;
+
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, ROS_PACKAGE_NAME);
+
+  ros::NodeHandle n;
+  ros::NodeHandle pn("~");
+
+  pn.param("map_frame", p_map_frame_, std::string("map"));
+  pn.param("base_footprint_frame", p_base_footprint_frame_, std::string("base_footprint"));
+  pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
+  pn.param("base_frame", p_base_frame_, std::string("base_link"));
+
+  fused_imu_msg_.header.frame_id = p_base_stabilized_frame_;
+  odom_msg_.header.frame_id = "map";
+
+  tfB_ = new tf::TransformBroadcaster();
+
+  fused_imu_publisher_ = n.advertise<sensor_msgs::Imu>("/fused_imu",1,false);
+  odometry_publisher_ = n.advertise<nav_msgs::Odometry>("/state", 1, false);
+
+  ros::Subscriber imu_subscriber = n.subscribe("/imu", 10, imuMsgCallback);
+  ros::Subscriber pose_subscriber = n.subscribe("/pose", 10, poseMsgCallback);
+
+  callback_count_ = 0;
+
+  ros::spin();
+
+  delete tfB_;
+
+  return 0;
+}

Localizations/Libraries/Ros/hector_slam/hector_map_server/CHANGELOG.rst

@@ -0,0 +1,43 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_map_server
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_map_server/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_map_server)
+
+find_package(catkin REQUIRED COMPONENTS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf)
+
+catkin_package(
+  CATKIN_DEPENDS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(hector_map_server src/hector_map_server.cpp)
+add_dependencies(hector_map_server ${catkin_EXPORTED_TARGETS})
+target_link_libraries(hector_map_server
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS hector_map_server
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

Localizations/Libraries/Ros/hector_slam/hector_map_server/package.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_map_server</name>
+  <version>0.5.2</version>
+  <description>
+    hector_map_server provides a service for retrieving the map, as well as for raycasting based obstacle queries (finds next obstacle in the map, given start and endpoint
+    in any tf coordinate frame).
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_map_server</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>hector_map_tools</build_depend>
+  <build_depend>hector_marker_drawing</build_depend>
+  <build_depend>hector_nav_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>hector_map_tools</run_depend>
+  <run_depend>hector_marker_drawing</run_depend>
+  <run_depend>hector_nav_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_map_server/src/hector_map_server.cpp

@@ -0,0 +1,331 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, Johannes Meyer TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 <stdio.h>
+#include <stdlib.h>
+//#include <libgen.h>
+//#include <fstream>
+
+#include "ros/ros.h"
+#include "ros/console.h"
+#include "nav_msgs/MapMetaData.h"
+#include "nav_msgs/OccupancyGrid.h"
+
+#include <boost/thread.hpp>
+
+#include <tf/transform_listener.h>
+
+#include "nav_msgs/GetMap.h"
+
+#include "hector_marker_drawing/HectorDrawings.h"
+#include "hector_map_tools/HectorMapTools.h"
+
+#include "hector_nav_msgs/GetDistanceToObstacle.h"
+#include "hector_nav_msgs/GetSearchPosition.h"
+
+
+class OccupancyGridContainer
+{
+public:
+  OccupancyGridContainer(std::string sub_topic, std::string prefix, ros::NodeHandle& nh, HectorDrawings* drawing_provider, tf::TransformListener* tf_)
+    : drawing_provider_(drawing_provider)
+    , tf_(tf_)
+  {
+
+    std::string service_name = "map";
+    map_service_ = nh.advertiseService(service_name, &OccupancyGridContainer::mapServiceCallback, this);
+
+    ros::NodeHandle pnh("~");
+    std::string lookup_service_name = "get_distance_to_obstacle";
+    dist_lookup_service_ = pnh.advertiseService(lookup_service_name, &OccupancyGridContainer::lookupServiceCallback, this);
+
+    std::string get_search_pos_service_name = "get_search_position";
+    get_search_pos_service_ = pnh.advertiseService(get_search_pos_service_name, &OccupancyGridContainer::getSearchPosServiceCallback, this);
+
+    map_sub_ = nh.subscribe("map", 1, &OccupancyGridContainer::mapCallback, this);
+  }
+
+  ~OccupancyGridContainer()
+  {}
+
+  bool mapServiceCallback(nav_msgs::GetMap::Request  &req,
+                          nav_msgs::GetMap::Response &res )
+  {
+    ROS_INFO("hector_map_server map service called");
+
+    if (!map_ptr_){
+      ROS_INFO("map_server has no map yet, no map service available");
+      return false;
+    }
+
+    res.map = *map_ptr_;
+
+    return true;
+  }
+
+  bool lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request  &req,
+                          hector_nav_msgs::GetDistanceToObstacle::Response &res )
+  {
+    //ROS_INFO("hector_map_server lookup service called");
+
+
+    if (!map_ptr_){
+      ROS_INFO("map_server has no map yet, no lookup service available");
+      return false;
+    }
+
+    tf::StampedTransform stamped_pose;
+
+
+
+    try{
+      tf_->waitForTransform(map_ptr_->header.frame_id,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0));
+      tf_->lookupTransform(map_ptr_->header.frame_id, req.point.header.frame_id, req.point.header.stamp, stamped_pose);
+
+      tf::Point v2_tf;
+      tf::pointMsgToTF(req.point.point,v2_tf);
+
+      tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
+      tf::Vector3 v2 = stamped_pose * v2_tf;
+      tf::Vector3 diff = v2 - v1;
+      v2 = v1 + diff / tf::Vector3(diff.x(), diff.y(), 0.0).length() * 5.0f;
+
+      Eigen::Vector2f start(v1.x(),v1.y());
+      Eigen::Vector2f end(v2.x(),v2.y());
+
+      Eigen::Vector2f hit_world;
+      //float dist = dist_meas_.getDist(start,end, &hit_world);
+      float dist = dist_meas_.getDist(start,end,&hit_world);
+
+      if (dist >=0.0f){
+        tf::Vector3 diff (v2-v1);
+
+        float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
+
+        res.distance = dist/cos(angle);
+
+      }else{
+        res.distance = -1.0f;
+      }
+
+      //debug drawing
+      if (false){
+
+        float cube_scale = map_ptr_->info.resolution;
+        drawing_provider_->setColor(1.0, 0.0, 0.0);
+        drawing_provider_->setScale(static_cast<double>(cube_scale));
+
+        drawing_provider_->drawPoint(start);
+
+        drawing_provider_->setColor(0.0, 1.0, 0.0);
+        drawing_provider_->drawPoint(end);
+
+        if (dist >= 0.0f){
+          drawing_provider_->setColor(0.0, 0.0, 1.0);
+          drawing_provider_->drawPoint(hit_world);
+        }
+
+        drawing_provider_->sendAndResetData();
+      }
+
+      return true;
+
+    }
+    catch(tf::TransformException e)
+    {
+      ROS_ERROR("Transform failed in lookup distance service call: %s",e.what());
+    }
+
+    return false;
+  }
+
+  bool getSearchPosServiceCallback(hector_nav_msgs::GetSearchPosition::Request  &req,
+                                   hector_nav_msgs::GetSearchPosition::Response &res )
+  {
+    if (!map_ptr_){
+      ROS_INFO("map_server has no map yet, no get best search pos service available");
+      return false;
+    }
+
+    try{
+
+      tf::Stamped<tf::Pose> ooi_pose, transformed_pose, search_position;
+      tf::poseStampedMsgToTF(req.ooi_pose, ooi_pose);
+
+      tf_->waitForTransform(map_ptr_->header.frame_id, req.ooi_pose.header.frame_id, req.ooi_pose.header.stamp, ros::Duration(1.0));
+      tf_->transformPose(map_ptr_->header.frame_id, ooi_pose, transformed_pose);
+
+      tf::Vector3 direction(-req.distance, 0.0, 0.0);
+      search_position = transformed_pose;
+      search_position.setOrigin(transformed_pose.getOrigin() + transformed_pose.getBasis() * direction);
+
+      tf::poseStampedTFToMsg(search_position, res.search_pose);
+
+      return true;
+
+//      //tf::Point v2_tf;
+//      //tf::pointMsgToTF(req.point.point,v2_tf);
+
+//      tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
+
+//      //warning: 3D!
+//      tf::Vector3 v2 = stamped_pose * tf::Vector3(-1.0, 0.0, 0.0);
+
+//      tf::Vector3 dir = v2-v1;
+
+//      Eigen::Vector2f dir_2d (dir.x(), dir.y());
+
+//      dir_2d.normalize();
+
+//      Eigen::Vector2f searchPos (Eigen::Vector2f(v1.x(),v1.y()) + (dir_2d*0.5f));
+
+//      //copy original pose message but set translation
+//      res.search_pose.pose = ooi_pose.pose;
+
+//      res.search_pose.pose.position.x = searchPos.x();
+//      res.search_pose.pose.position.y = searchPos.y();
+
+      //return true;
+
+
+      //Eigen::Vector2f ooi_pos(v1.x(),v1.y());
+      //Eigen::Vector2f sample_point_pos(v2.x(),v2.y());
+
+
+      //float dist_from_target = dist_meas_.getDist(ooi_pos,sample_point_pos);
+      //float dist_from_sample_point = dist_meas_.getDist(sample_point_pos, ooi_pos);
+
+
+
+/*
+      if (dist >=0.0f){
+        tf::Vector3 diff (v2-v1);
+
+        float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
+
+        res.distance = dist/cos(angle);
+
+
+        //debug drawing
+        if (true){
+
+          float cube_scale = map_ptr_->info.resolution;
+          drawing_provider_->setColor(1.0, 0.0, 0.0);
+          drawing_provider_->setScale(static_cast<double>(cube_scale));
+
+          drawing_provider_->drawPoint(start);
+          drawing_provider_->drawPoint(end);
+
+          if (dist >= 0.0f){
+            drawing_provider_->setColor(0.0, 0.0, 1.0);
+            drawing_provider_->drawPoint(hit_world);
+          }
+
+          drawing_provider_->sendAndResetData();
+        }
+
+      }
+      */
+      //return true;
+
+    }    catch(tf::TransformException e){
+      ROS_ERROR("Transform failed in getSearchPosition service call: %s",e.what());
+    }
+
+    return false;
+  }
+
+  void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
+  {
+    map_ptr_ = map;
+
+    dist_meas_.setMap(map_ptr_);
+  }
+
+  //Services
+  ros::ServiceServer map_service_;
+  ros::ServiceServer dist_lookup_service_;
+  ros::ServiceServer get_search_pos_service_;
+
+  //Subscriber
+  ros::Subscriber map_sub_;
+
+  HectorMapTools::DistanceMeasurementProvider dist_meas_;
+
+  HectorDrawings* drawing_provider_;
+  tf::TransformListener* tf_;
+
+  //nav_msgs::MapMetaData meta_data_message_;
+  nav_msgs::GetMap::Response map_resp_;
+
+  nav_msgs::OccupancyGridConstPtr map_ptr_;
+};
+
+class HectorMapServer
+{
+  public:
+    /** Trivial constructor */
+    HectorMapServer(ros::NodeHandle& private_nh)
+    {
+      std::string frame_id;
+
+      hector_drawings_ = new HectorDrawings();
+      hector_drawings_->setNamespace("map_server");
+
+      mapContainer = new OccupancyGridContainer("map", "" ,private_nh, hector_drawings_,&tf_);
+    }
+
+    ~HectorMapServer()
+    {
+      delete mapContainer;
+
+
+      delete hector_drawings_;
+    }
+
+
+public:
+    OccupancyGridContainer* mapContainer;
+private:
+
+    HectorDrawings* hector_drawings_;
+    tf::TransformListener tf_;
+};
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "hector_map_server");
+  ros::NodeHandle nh;
+
+  HectorMapServer ms(nh);
+
+  ros::spin();
+
+  return 0;
+}
+

Localizations/Libraries/Ros/hector_slam/hector_map_tools/CHANGELOG.rst

@@ -0,0 +1,51 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_map_tools
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+* hector_map_tools: Use the FindEigen3.cmake module provided by Eigen
+  This patch applies the recommendation from http://wiki.ros.org/jade/Migration and removes the
+  dependency from package cmake_modules (unless your installation of Eigen3 does not provide a
+  cmake config).
+  Same as 1251d9dc20854f48da116eed25780c103a5bd003, but package hector_map_tools was not updated
+  back then.
+* Contributors: Johannes Meyer
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+* -Fix severe bug when not using square size maps (would results in completely wrong obstacle distances and coordinates)
+* Contributors: Stefan Kohlbrecher
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_map_tools/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_map_tools)
+
+find_package(catkin REQUIRED COMPONENTS nav_msgs)
+
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS nav_msgs
+  DEPENDS EIGEN3
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

Localizations/Libraries/Ros/hector_slam/hector_map_tools/include/hector_map_tools/HectorMapTools.h

@@ -0,0 +1,293 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __HectorMapTools_h_
+#define __HectorMapTools_h_
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
+
+#include<Eigen/Core>
+
+class HectorMapTools{
+public:
+
+  template<typename ConcreteScalar>
+  class CoordinateTransformer{
+  public:
+
+    CoordinateTransformer()
+    {
+
+    }
+
+    CoordinateTransformer(const nav_msgs::OccupancyGridConstPtr map)
+    {
+      this->setTransforms(*map);
+    }
+
+
+    void setTransforms(const nav_msgs::OccupancyGrid& map)
+    {
+      this->setTransforms(map.info);
+    }
+
+    void setTransforms(const nav_msgs::MapMetaData& meta)
+    {
+      origo_ = (Eigen::Matrix<ConcreteScalar, 2, 1>(static_cast<ConcreteScalar>(meta.origin.position.x),static_cast<ConcreteScalar>(meta.origin.position.y)));
+      scale_ = (static_cast<ConcreteScalar>(meta.resolution));
+      inv_scale_ = (static_cast<ConcreteScalar>(1.0f / meta.resolution));
+    }
+
+    void setTransformsBetweenCoordSystems(const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS1, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS1,
+                                          const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS2, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS2)
+    {
+      Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_x_factors = getLinearTransform(Eigen::Vector2f(origoCS1[0], endCS1[0]), Eigen::Vector2f(origoCS2[0], endCS2[0]));
+      Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_y_factors = getLinearTransform(Eigen::Vector2f(origoCS1[1], endCS1[1]), Eigen::Vector2f(origoCS2[1], endCS2[1]));
+
+      //std::cout << "\n geo_x: \n" << map_t_geotiff_x_factors << "\n geo_y: \n" << map_t_geotiff_y_factors << "\n";
+
+      origo_.x() = map_t_geotiff_x_factors[1];
+      origo_.y() = map_t_geotiff_y_factors[1];
+
+      scale_ = map_t_geotiff_x_factors[0];
+      inv_scale_ = 1.0 / scale_;
+
+      //std::cout << "\nscale " << scale_ << "\n";
+    }
+
+    Eigen::Matrix<ConcreteScalar, 2, 1> getC1Coords(const  Eigen::Matrix<ConcreteScalar, 2, 1>& mapCoords) const
+    {
+      return origo_ + (mapCoords * scale_);
+    }
+
+    Eigen::Matrix<ConcreteScalar, 2, 1> getC2Coords(const  Eigen::Matrix<ConcreteScalar, 2, 1>& worldCoords) const
+    {
+      return ((worldCoords - origo_) * inv_scale_);
+    }
+
+    ConcreteScalar getC1Scale(float c2_scale) const
+    {
+      return scale_ * c2_scale;
+    }
+
+    ConcreteScalar getC2Scale(float c1_scale) const
+    {
+      return inv_scale_ * c1_scale;
+    }
+
+  protected:
+
+    Eigen::Matrix<ConcreteScalar, 2, 1> getLinearTransform(const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem1, const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem2)
+    {
+      ConcreteScalar scaling = (coordSystem2[0] - coordSystem2[1]) / (coordSystem1[0] - coordSystem1[1]);
+      ConcreteScalar translation = coordSystem2[0] - coordSystem1[0] * scaling;
+      return Eigen::Vector2f (scaling, translation);
+    }
+
+    Eigen::Matrix<ConcreteScalar, 2, 1> origo_;
+    ConcreteScalar scale_;
+    ConcreteScalar inv_scale_;
+  };
+
+  class DistanceMeasurementProvider
+  {
+  public:
+    DistanceMeasurementProvider()
+    {
+
+    }
+
+    void setMap(const nav_msgs::OccupancyGridConstPtr map)
+    {
+      map_ptr_ = map;
+
+      world_map_transformer_.setTransforms(*map_ptr_);
+    }
+
+    float getDist(const Eigen::Vector2f& begin_world, const Eigen::Vector2f& end_world, Eigen::Vector2f* hitCoords = 0)
+    {
+      Eigen::Vector2i end_point_map;
+
+      Eigen::Vector2i begin_map (world_map_transformer_.getC2Coords(begin_world).cast<int>());
+      Eigen::Vector2i end_map (world_map_transformer_.getC2Coords(end_world).cast<int>());
+      float dist = checkOccupancyBresenhami(begin_map, end_map, &end_point_map);
+
+      if (hitCoords != 0){
+        *hitCoords = world_map_transformer_.getC1Coords(end_point_map.cast<float>());
+      }
+
+      return world_map_transformer_.getC1Scale(dist);
+    }
+
+    inline float checkOccupancyBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, Eigen::Vector2i* hitCoords = 0, unsigned int max_length = UINT_MAX){
+
+      int x0 = beginMap[0];
+      int y0 = beginMap[1];
+
+      int sizeX = map_ptr_->info.width;
+      int sizeY = map_ptr_->info.height;
+
+      //check if beam start point is inside map, cancel update if this is not the case
+      if ((x0 < 0) || (x0 >= sizeX) || (y0 < 0) || (y0 >= sizeY)) {
+        return -1.0f;
+      }
+
+      int x1 = endMap[0];
+      int y1 = endMap[1];
+
+      //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";
+
+      //check if beam end point is inside map, cancel update if this is not the case
+      if ((x1 < 0) || (x1 >= sizeX) || (y1 < 0) || (y1 >= sizeY)) {
+        return -1.0f;
+      }
+
+      int dx = x1 - x0;
+      int dy = y1 - y0;
+
+      unsigned int abs_dx = abs(dx);
+      unsigned int abs_dy = abs(dy);
+
+      int offset_dx =  dx > 0 ? 1 : -1;
+      int offset_dy = (dy > 0 ? 1 : -1) * sizeX;
+
+      unsigned int startOffset = beginMap.y() * sizeX + beginMap.x();
+
+      int end_offset;
+
+      //if x is dominant
+      if(abs_dx >= abs_dy){
+        int error_y = abs_dx / 2;
+        end_offset = bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset,5000);
+      }else{
+        //otherwise y is dominant
+        int error_x = abs_dy / 2;
+        end_offset = bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset,5000);
+      }
+
+      if (end_offset != -1){
+        Eigen::Vector2i end_coords(end_offset % sizeX, end_offset / sizeX);
+
+        int distMap = ((beginMap - end_coords).cast<float>()).norm();
+
+        if (hitCoords != 0){
+          *hitCoords = end_coords;
+        }
+
+        return distMap;
+      }
+
+      return -1.0f;
+
+      //unsigned int endOffset = endMap.y() * sizeX + endMap.x();
+      //this->bresenhamCellOcc(endOffset);
+    }
+
+    inline int bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
+                            unsigned int offset, unsigned int max_length){
+      unsigned int end = std::min(max_length, abs_da);
+
+      const std::vector<int8_t>& data = map_ptr_->data;
+
+      for(unsigned int i = 0; i < end; ++i){
+        if (data[offset] == 100){
+          return static_cast<int>(offset);
+        }
+
+        offset += offset_a;
+        error_b += abs_db;
+        if((unsigned int)error_b >= abs_da){
+          offset += offset_b;
+          error_b -= abs_da;
+        }
+      }
+      return -1;
+      //at(offset);
+    }
+
+  protected:
+    CoordinateTransformer<float> world_map_transformer_;
+    nav_msgs::OccupancyGridConstPtr map_ptr_;
+
+
+  };
+
+  static bool getMapExtends(const nav_msgs::OccupancyGrid& map, Eigen::Vector2i& topLeft, Eigen::Vector2i& bottomRight)
+  {
+    int lowerStart = -1;
+    int upperStart = 10000000;
+
+    int xMaxTemp = lowerStart;
+    int yMaxTemp = lowerStart;
+    int xMinTemp = upperStart;
+    int yMinTemp = upperStart;
+
+    int width = map.info.width;
+    int height = map.info.height;
+
+    for (int y = 0; y < height; ++y){
+      for (int x = 0; x < width; ++x){
+
+        if ( map.data[x+y*width] != -1){
+
+          if (x > xMaxTemp) {
+            xMaxTemp = x;
+          }
+
+          if (x < xMinTemp) {
+            xMinTemp = x;
+          }
+
+          if (y > yMaxTemp) {
+            yMaxTemp = y;
+          }
+
+          if (y < yMinTemp) {
+            yMinTemp = y;
+          }
+        }
+      }
+    }
+
+    if ((xMaxTemp != lowerStart) &&
+        (yMaxTemp != lowerStart) &&
+        (xMinTemp != upperStart) &&
+        (yMinTemp != upperStart)) {
+
+      topLeft = Eigen::Vector2i(xMinTemp,yMinTemp);
+      bottomRight = Eigen::Vector2i(xMaxTemp+1, yMaxTemp+1);
+
+      return true;
+    } else {
+      return false;
+    }
+  };
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_map_tools/package.xml

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_map_tools</name>
+  <version>0.5.2</version>
+  <description>
+    hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps.
+    Currently consists of a single header.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_map_tools</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>eigen</build_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>eigen</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_mapping/CHANGELOG.rst

@@ -0,0 +1,64 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_mapping
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+* Remove tf_conversions as a dependency (`#93 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/93>`_)
+* Add reset mapping (and pose) service (`#87 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/87>`_)
+  * add service to reset the mapping with a new initial pose
+* Reorganize scanCallback and add comments (`#89 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/89>`_)
+  * Refactor, reformat and comment scanCallback
+  * make rosPointCloudToDataContainer void
+  * make rosLaserScanToDataContainer void
+* Add pause and reset services to hector_mapping (`#86 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/86>`_)
+  * Add pause and reset services to Hector
+* Contributors: Marcelino Almeida
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* fixed compilation under noetic
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+* Remove unnecessary boost signals find_package
+  With Boost >1.69 hector_mapping won't build. Furthermore, hector_mapping doesn't use signals anywhere.
+* Contributors: Sam Pfeiffer
+
+0.3.6 (2019-10-31)
+------------------
+* Merge pull request `#49 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/49>`_ from davidbsp/catkin
+  populate child_frame_id in odometry msg
+* Added child_frame_id in hector mapping's odometry msg
+* Contributors: David Portugal, Johannes Meyer
+
+0.3.5 (2016-06-24)
+------------------
+* Use the FindEigen3.cmake module provided by Eigen
+* Contributors: Johannes Meyer
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* respect ``p_map_frame_``
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam
+* hector_mapping: fixed multi-resolution map scan matching index bug in MapRepMultiMap.h

Localizations/Libraries/Ros/hector_slam/hector_mapping/CMakeLists.txt

@@ -0,0 +1,112 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_mapping)
+
+find_package(catkin REQUIRED COMPONENTS 
+  roscpp
+  nav_msgs
+  visualization_msgs
+  tf
+  message_filters
+  laser_geometry
+  message_generation
+  std_srvs)
+
+find_package(Boost REQUIRED COMPONENTS thread)
+
+find_package(Eigen3 REQUIRED)
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  HectorDebugInfo.msg
+  HectorIterData.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  ResetMapping.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES hector_mapping_lib
+  CATKIN_DEPENDS roscpp nav_msgs visualization_msgs tf message_filters laser_geometry message_runtime
+  #DEPENDS EIGEN3
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library( ${PROJECT_NAME}_lib
+  src/PoseInfoContainer.cpp
+  src/HectorMappingRos.cpp
+)
+
+add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_lib
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${EIGEN3_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME} src/main.cpp)
+add_dependencies(${PROJECT_NAME}   
+  ${${PROJECT_NAME}_EXPORTED_TARGETS} 
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PROJECT_NAME}_lib
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(TARGETS ${PROJECT_NAME}_lib
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS hector_mapping
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Don't ask why it's hector_mapping, this was Stefan Kohlbrecher's first ROS package and a wrapper of a pre ROS header only library
+install(DIRECTORY include
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
+)

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDebugInfoProvider.h

@@ -0,0 +1,94 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 HECTOR_DEBUG_INFO_PROVIDER_H__
+#define HECTOR_DEBUG_INFO_PROVIDER_H__
+
+#include "util/HectorDebugInfoInterface.h"
+#include "util/UtilFunctions.h"
+
+#include "ros/ros.h"
+
+#include "hector_mapping/HectorDebugInfo.h"
+
+
+class HectorDebugInfoProvider : public HectorDebugInfoInterface
+{
+public:
+
+  HectorDebugInfoProvider()
+  {
+    ros::NodeHandle nh_;
+
+    debugInfoPublisher_ = nh_.advertise<hector_mapping::HectorDebugInfo>("hector_debug_info", 50, true);
+  };
+
+  virtual void sendAndResetData()
+  {
+    debugInfoPublisher_.publish(debugInfo);
+    debugInfo.iterData.clear();
+  }
+
+
+  virtual void addHessianMatrix(const Eigen::Matrix3f& hessian)
+  {
+    hector_mapping::HectorIterData iterData;
+
+    for (int i=0; i < 9; ++i){
+      iterData.hessian[i] = static_cast<double>(hessian.data()[i]);
+      iterData.determinant = hessian.determinant();
+
+      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(hessian);
+
+      const Eigen::Vector3f& eigValues (eig.eigenvalues());
+      iterData.conditionNum = eigValues[2] / eigValues[0];
+
+
+      iterData.determinant2d = hessian.block<2,2>(0,0).determinant();
+      Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig2d(hessian.block<2,2>(0,0));
+
+      const Eigen::Vector2f& eigValues2d (eig2d.eigenvalues());
+      iterData.conditionNum2d = eigValues2d[1] / eigValues2d[0];
+    }
+
+    debugInfo.iterData.push_back(iterData);
+  }
+
+  virtual void addPoseLikelihood(float lh)
+  {
+
+  }
+
+
+  hector_mapping::HectorDebugInfo debugInfo;
+
+  ros::Publisher debugInfoPublisher_;
+
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDrawings.h

@@ -0,0 +1,171 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 HECTOR_DRAWINGS_H__
+#define HECTOR_DRAWINGS_H__
+
+#include "util/DrawInterface.h"
+#include "util/UtilFunctions.h"
+
+#include "ros/ros.h"
+
+#include <visualization_msgs/MarkerArray.h>
+
+#include <Eigen/Dense>
+
+
+class HectorDrawings : public DrawInterface
+{
+public:
+
+  HectorDrawings()
+  {
+    idCounter = 0;
+
+    ros::NodeHandle nh_;
+
+    markerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1, true);
+    markerArrayPublisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1, true);
+
+    tempMarker.header.frame_id = "map";
+    tempMarker.ns = "slam";
+
+    this->setScale(1.0);
+    this->setColor(1.0, 1.0, 1.0);
+
+    tempMarker.action = visualization_msgs::Marker::ADD;
+  };
+
+  virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame)
+  {
+    tempMarker.id = idCounter++;
+
+    tempMarker.pose.position.x = pointWorldFrame.x();
+    tempMarker.pose.position.y = pointWorldFrame.y();
+
+    tempMarker.pose.orientation.w = 0.0;
+    tempMarker.pose.orientation.z = 0.0;
+    tempMarker.type = visualization_msgs::Marker::CUBE;
+
+    //markerPublisher_.publish(tempMarker);
+
+    markerArray.markers.push_back(tempMarker);
+  }
+
+  virtual void drawArrow(const Eigen::Vector3f& poseWorld)
+  {
+    tempMarker.id = idCounter++;
+
+    tempMarker.pose.position.x = poseWorld.x();
+    tempMarker.pose.position.y = poseWorld.y();
+
+    tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f);
+    tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f);
+
+    tempMarker.type = visualization_msgs::Marker::ARROW;
+
+    //markerPublisher_.publish(tempMarker);
+
+    markerArray.markers.push_back(tempMarker);
+
+  }
+
+  virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix)
+  {
+    tempMarker.pose.position.x = mean[0];
+    tempMarker.pose.position.y = mean[1];
+
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covMatrix);
+
+    const Eigen::Vector2f& eigValues (eig.eigenvalues());
+    const Eigen::Matrix2f& eigVectors (eig.eigenvectors());
+
+    float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));
+
+    tempMarker.type = visualization_msgs::Marker::CYLINDER;
+
+    double lengthMajor = sqrt(eigValues[0]);
+    double lengthMinor = sqrt(eigValues[1]);
+
+    tempMarker.scale.x = lengthMajor;
+    tempMarker.scale.y = lengthMinor;
+    tempMarker.scale.z = 0.001;
+
+
+    tempMarker.pose.orientation.w = cos(angle*0.5);
+    tempMarker.pose.orientation.z = sin(angle*0.5);
+
+    tempMarker.id = idCounter++;
+    markerArray.markers.push_back(tempMarker);
+
+    //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(lengthMajor ,0,0));
+    //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,lengthMinor,0));
+
+    //glScalef(lengthMajor, lengthMinor, 0);
+    //glCallList(dlCircle);
+    //this->popCS();
+  }
+
+  virtual void setScale(double scale)
+  {
+    tempMarker.scale.x = scale;
+    tempMarker.scale.y = scale;
+    tempMarker.scale.z = scale;
+  }
+
+  virtual void setColor(double r, double g, double b, double a = 1.0)
+  {
+    tempMarker.color.r = r;
+    tempMarker.color.g = g;
+    tempMarker.color.b = b;
+    tempMarker.color.a = a;
+  }
+
+  virtual void sendAndResetData()
+  {
+    markerArrayPublisher_.publish(markerArray);
+    markerArray.markers.clear();
+    idCounter = 0;
+  }
+
+  void setTime(const ros::Time& time)
+  {
+    tempMarker.header.stamp = time;
+  }
+
+
+  ros::Publisher markerPublisher_;
+  ros::Publisher markerArrayPublisher_;
+
+  visualization_msgs::Marker tempMarker;
+  visualization_msgs::MarkerArray markerArray;
+
+  int idCounter;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMapMutex.h

@@ -0,0 +1,24 @@
+#ifndef hectormapmutex_h__
+#define hectormapmutex_h__
+
+#include "util/MapLockerInterface.h"
+
+#include <boost/thread/mutex.hpp>
+
+class HectorMapMutex : public MapLockerInterface
+{
+public:
+  virtual void lockMap()
+  {
+    mapModifyMutex_.lock();
+  }
+
+  virtual void unlockMap()
+  {
+    mapModifyMutex_.unlock();
+  }
+
+  boost::mutex mapModifyMutex_;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMappingRos.h

@@ -0,0 +1,208 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 HECTOR_MAPPING_ROS_H__
+#define HECTOR_MAPPING_ROS_H__
+
+#include "ros/ros.h"
+
+#include "tf/transform_listener.h"
+#include "tf/transform_broadcaster.h"
+#include "tf/message_filter.h"
+#include "message_filters/subscriber.h"
+
+#include "sensor_msgs/LaserScan.h"
+#include <std_msgs/String.h>
+
+#include <hector_mapping/ResetMapping.h>
+#include <std_srvs/SetBool.h>
+#include <std_srvs/Trigger.h>
+
+#include "laser_geometry/laser_geometry.h"
+#include "nav_msgs/GetMap.h"
+
+#include "slam_main/HectorSlamProcessor.h"
+
+#include "scan/DataPointContainer.h"
+#include "util/MapLockerInterface.h"
+
+#include <boost/thread.hpp>
+
+#include "PoseInfoContainer.h"
+
+
+class HectorDrawings;
+class HectorDebugInfoProvider;
+
+class MapPublisherContainer
+{
+public:
+  ros::Publisher mapPublisher_;
+  ros::Publisher mapMetadataPublisher_;
+  nav_msgs::GetMap::Response map_;
+  ros::ServiceServer dynamicMapServiceServer_;
+};
+
+class HectorMappingRos
+{
+public:
+  HectorMappingRos();
+  ~HectorMappingRos();
+
+
+  void scanCallback(const sensor_msgs::LaserScan& scan);
+  void sysMsgCallback(const std_msgs::String& string);
+
+  bool mapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+  bool resetMapCallback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res);
+  bool restartHectorCallback(hector_mapping::ResetMapping::Request  &req, hector_mapping::ResetMapping::Response &res);
+  bool pauseMapCallback(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res);
+
+  void publishMap(MapPublisherContainer& map_, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex = 0);
+
+  void rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap);
+  void rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap);
+
+  void setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap);
+
+  void publishTransformLoop(double p_transform_pub_period_);
+  void publishMapLoop(double p_map_pub_period_);
+  void publishTransform();
+
+  void staticMapCallback(const nav_msgs::OccupancyGrid& map);
+  void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
+
+  // Internal mapping management functions
+  void toggleMappingPause(bool pause);
+  void resetPose(const geometry_msgs::Pose &pose);
+
+  /*
+  void setStaticMapData(const nav_msgs::OccupancyGrid& map);
+  */
+protected:
+
+  HectorDebugInfoProvider* debugInfoProvider;
+  HectorDrawings* hectorDrawings;
+
+  int lastGetMapUpdateIndex;
+
+  ros::NodeHandle node_;
+
+  ros::Subscriber scanSubscriber_;
+  ros::Subscriber sysMsgSubscriber_;
+
+  ros::Subscriber mapSubscriber_;
+  message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_sub_;
+  tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_filter_;
+
+  ros::Publisher posePublisher_;
+  ros::Publisher poseUpdatePublisher_;
+  ros::Publisher twistUpdatePublisher_;
+  ros::Publisher odometryPublisher_;
+  ros::Publisher scan_point_cloud_publisher_;
+
+  ros::ServiceServer reset_map_service_;
+  ros::ServiceServer restart_hector_service_;
+  ros::ServiceServer toggle_scan_processing_service_;
+
+  std::vector<MapPublisherContainer> mapPubContainer;
+
+  tf::TransformListener tf_;
+  tf::TransformBroadcaster* tfB_;
+
+  laser_geometry::LaserProjection projector_;
+
+  tf::Transform map_to_odom_;
+
+  boost::thread* map__publish_thread_;
+
+  hectorslam::HectorSlamProcessor* slamProcessor;
+  hectorslam::DataContainer laserScanContainer;
+
+  PoseInfoContainer poseInfoContainer_;
+
+  sensor_msgs::PointCloud laser_point_cloud_;
+
+  ros::Time lastMapPublishTime;
+  ros::Time lastScanTime;
+  Eigen::Vector3f lastSlamPose;
+
+  bool initial_pose_set_;
+  Eigen::Vector3f initial_pose_;
+
+  bool pause_scan_processing_;
+
+  //-----------------------------------------------------------
+  // Parameters
+
+  std::string p_base_frame_;
+  std::string p_map_frame_;
+  std::string p_odom_frame_;
+
+  //Parameters related to publishing the scanmatcher pose directly via tf
+  bool p_pub_map_scanmatch_transform_;
+  std::string p_tf_map_scanmatch_transform_frame_name_;
+
+  std::string p_scan_topic_;
+  std::string p_sys_msg_topic_;
+
+  std::string p_pose_update_topic_;
+  std::string p_twist_update_topic_;
+
+  bool p_pub_drawings;
+  bool p_pub_debug_output_;
+  bool p_pub_map_odom_transform_;
+  bool p_pub_odometry_;
+  bool p_advertise_map_service_;
+  int p_scan_subscriber_queue_size_;
+
+  double p_update_factor_free_;
+  double p_update_factor_occupied_;
+  double p_map_update_distance_threshold_;
+  double p_map_update_angle_threshold_;
+
+  double p_map_resolution_;
+  int p_map_size_;
+  double p_map_start_x_;
+  double p_map_start_y_;
+  int p_map_multi_res_levels_;
+
+  double p_map_pub_period_;
+
+  bool p_use_tf_scan_transformation_;
+  bool p_use_tf_pose_start_estimate_;
+  bool p_map_with_known_poses_;
+  bool p_timing_output_;
+
+  float p_sqr_laser_min_dist_;
+  float p_sqr_laser_max_dist_;
+  float p_laser_z_min_value_;
+  float p_laser_z_max_value_;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/PoseInfoContainer.h

@@ -0,0 +1,55 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 POSE_INFO_CONTAINER_H__
+#define POSE_INFO_CONTAINER_H__
+
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <Eigen/Core>
+
+class PoseInfoContainer{
+public:
+
+  void update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id);
+
+  const geometry_msgs::PoseStamped& getPoseStamped() { return stampedPose_; };
+  const geometry_msgs::PoseWithCovarianceStamped& getPoseWithCovarianceStamped() { return covPose_; };
+  const tf::Transform& getTfTransform() { return poseTransform_; };
+
+protected:
+  geometry_msgs::PoseStamped stampedPose_;
+  geometry_msgs::PoseWithCovarianceStamped covPose_;
+  tf::Transform poseTransform_;
+
+};
+
+#endif
+

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMap.h

@@ -0,0 +1,45 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMap_h_
+#define __GridMap_h_
+
+#include "OccGridMapBase.h"
+#include "GridMapLogOdds.h"
+#include "GridMapReflectanceCount.h"
+#include "GridMapSimpleCount.h"
+
+namespace hectorslam {
+
+typedef OccGridMapBase<LogOddsCell, GridMapLogOddsFunctions> GridMap;
+//typedef OccGridMapBase<SimpleCountCell, GridMapSimpleCountFunctions> GridMap;
+//typedef OccGridMapBase<ReflectanceCell, GridMapReflectanceFunctions> GridMap;
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapBase.h

@@ -0,0 +1,404 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMapBase_h_
+#define __GridMapBase_h_
+
+#include <Eigen/Geometry>
+#include <Eigen/LU>
+
+#include "MapDimensionProperties.h"
+
+namespace hectorslam {
+
+/**
+ * GridMapBase provides basic grid map functionality (creates grid , provides transformation from/to world coordinates).
+ * It serves as the base class for different map representations that may extend it's functionality.
+ */
+template<typename ConcreteCellType>
+class GridMapBase
+{
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * Indicates if given x and y are within map bounds
+   * @return True if coordinates are within map bounds
+   */
+  bool hasGridValue(int x, int y) const
+  {
+    return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY());
+  }
+
+  const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); };
+  int getSizeX() const { return mapDimensionProperties.getSizeX(); };
+  int getSizeY() const { return mapDimensionProperties.getSizeY(); };
+
+  bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const
+  {
+    return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords);
+  }
+
+  virtual void reset()
+  {
+    this->clear();
+  }
+
+  /**
+   * Resets the grid cell values by using the resetGridCell() function.
+   */
+  void clear()
+  {
+    int size = this->getSizeX() * this->getSizeY();
+
+    for (int i = 0; i < size; ++i) {
+      this->mapArray[i].resetGridCell();
+    }
+
+    //this->mapArray[0].set(1.0f);
+    //this->mapArray[size-1].set(1.0f);
+  }
+
+
+  const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; };
+
+  /**
+   * Constructor, creates grid representation and transformations.
+   */
+  GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
+    : mapArray(0)
+    , lastUpdateIndex(-1)
+  {
+    Eigen::Vector2i newMapDimensions (size);
+
+    this->setMapGridSize(newMapDimensions);
+    sizeX = size[0];
+
+    setMapTransformation(offset, mapResolution);
+
+    this->clear();
+  }
+
+  /**
+   * Destructor
+   */
+  virtual ~GridMapBase()
+  {
+    deleteArray();
+  }
+
+  /**
+   * Allocates memory for the two dimensional pointer array for map representation.
+   */
+  void allocateArray(const Eigen::Vector2i& newMapDims)
+  {
+    int sizeX = newMapDims.x();
+    int sizeY = newMapDims.y();
+
+    mapArray = new ConcreteCellType [sizeX*sizeY];
+
+    mapDimensionProperties.setMapCellDims(newMapDims);
+  }
+
+  void deleteArray()
+  {
+    if (mapArray != 0){
+
+      delete[] mapArray;
+
+      mapArray = 0;
+      mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1));
+    }
+  }
+
+  ConcreteCellType& getCell(int x, int y)
+  {
+    return mapArray[y * sizeX + x];
+  }
+
+  const ConcreteCellType& getCell(int x, int y) const
+  {
+    return mapArray[y * sizeX + x];
+  }
+
+  ConcreteCellType& getCell(int index)
+  {
+    return mapArray[index];
+  }
+
+  const ConcreteCellType& getCell(int index) const
+  {
+    return mapArray[index];
+  }
+
+  void setMapGridSize(const Eigen::Vector2i& newMapDims)
+  {
+    if (newMapDims != mapDimensionProperties.getMapDimensions() ){
+     deleteArray();
+     allocateArray(newMapDims);
+     this->reset();
+    }
+  }
+
+  /**
+   * Copy Constructor, only needed if pointer members are present.
+   */
+  GridMapBase(const GridMapBase& other)
+  {
+    allocateArray(other.getMapDimensions());
+    *this = other;
+  }
+
+  /**
+   * Assignment operator, only needed if pointer members are present.
+   */
+  GridMapBase& operator=(const GridMapBase& other)
+  {
+    if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){
+      this->setMapGridSize(other.mapDimensionProperties.getMapDimensions());
+    }
+
+    this->mapDimensionProperties = other.mapDimensionProperties;
+
+    this->worldTmap = other.worldTmap;
+    this->mapTworld = other.mapTworld;
+    this->worldTmap3D = other.worldTmap3D;
+
+    this->scaleToMap = other.scaleToMap;
+
+    //@todo potential resize
+    int sizeX = this->getSizeX();
+    int sizeY = this->getSizeY();
+
+    size_t concreteCellSize = sizeof(ConcreteCellType);
+
+    memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize);
+
+    return *this;
+  }
+
+  /**
+   * Returns the world coordinates for the given map coords.
+   */
+  inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const
+  {
+    return worldTmap * mapCoords;
+  }
+
+  /**
+   * Returns the map coordinates for the given world coords.
+   */
+  inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const
+  {
+    return mapTworld * worldCoords;
+  }
+
+  /**
+   * Returns the world pose for the given map pose.
+   */
+  inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
+  {
+    Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>());
+    return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
+  }
+
+  /**
+   * Returns the map pose for the given world pose.
+   */
+  inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
+  {
+    Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>());
+    return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
+  }
+
+  void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
+  {
+    setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn));
+  }
+
+  void setDimensionProperties(const MapDimensionProperties& newMapDimProps)
+  {
+    //Grid map cell number has changed
+    if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
+      this->setMapGridSize(newMapDimProps.getMapDimensions());
+    }
+
+    //Grid map transformation/cell size has changed
+    if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
+      this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
+    }
+  }
+
+  /**
+   * Set the map transformations
+   * @param xWorld The origin of the map coordinate system on the x axis in world coordinates
+   * @param yWorld The origin of the map coordinate system on the y axis in world coordinates
+   * @param The cell length of the grid map
+   */
+  void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength)
+  {
+    mapDimensionProperties.setCellLength(cellLength);
+    mapDimensionProperties.setTopLeftOffset(topLeftOffset);
+
+    scaleToMap = 1.0f / cellLength;
+
+    mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);
+
+    worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0);
+
+    //std::cout << worldTmap3D.matrix() << std::endl;
+    worldTmap3D = worldTmap3D.inverse();
+
+    worldTmap = mapTworld.inverse();
+  }
+
+
+  /**
+   * Returns the scale factor for one unit in world coords to one unit in map coords.
+   * @return The scale factor
+   */
+  float getScaleToMap() const
+  {
+    return scaleToMap;
+  }
+
+  /**
+   * Returns the cell edge length of grid cells in millimeters.
+   * @return the cell edge length in millimeters.
+   */
+  float getCellLength() const
+  {
+    return mapDimensionProperties.getCellLength();
+  }
+
+  /**
+   * Returns a reference to the homogenous 2D transform from map to world coordinates.
+   * @return The homogenous 2D transform.
+   */
+  const Eigen::Affine2f& getWorldTmap() const
+  {
+    return worldTmap;
+  }
+
+  /**
+   * Returns a reference to the homogenous 3D transform from map to world coordinates.
+   * @return The homogenous 3D transform.
+   */
+  const Eigen::Affine3f& getWorldTmap3D() const
+  {
+    return worldTmap3D;
+  }
+
+  /**
+   * Returns a reference to the homogenous 2D transform from world to map coordinates.
+   * @return The homogenous 2D transform.
+   */
+  const Eigen::Affine2f& getMapTworld() const
+  {
+    return mapTworld;
+  }
+
+  void setUpdated() { lastUpdateIndex++; };
+  int getUpdateIndex() const { return lastUpdateIndex; };
+
+  /**
+    * Returns the rectangle ([xMin,yMin],[xMax,xMax]) containing non-default cell values
+    */
+  bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const
+  {
+    int lowerStart = -1;
+    int upperStart = 10000;
+
+    int xMaxTemp = lowerStart;
+    int yMaxTemp = lowerStart;
+    int xMinTemp = upperStart;
+    int yMinTemp = upperStart;
+
+    int sizeX = this->getSizeX();
+    int sizeY = this->getSizeY();
+
+    for (int x = 0; x < sizeX; ++x) {
+      for (int y = 0; y < sizeY; ++y) {
+        if (this->mapArray[x][y].getValue() != 0.0f) {
+
+          if (x > xMaxTemp) {
+            xMaxTemp = x;
+          }
+
+          if (x < xMinTemp) {
+            xMinTemp = x;
+          }
+
+          if (y > yMaxTemp) {
+            yMaxTemp = y;
+          }
+
+          if (y < yMinTemp) {
+            yMinTemp = y;
+          }
+        }
+      }
+    }
+
+    if ((xMaxTemp != lowerStart) &&
+        (yMaxTemp != lowerStart) &&
+        (xMinTemp != upperStart) &&
+        (yMinTemp != upperStart)) {
+
+      xMax = xMaxTemp;
+      yMax = yMaxTemp;
+      xMin = xMinTemp;
+      yMin = yMinTemp;
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+protected:
+
+  ConcreteCellType *mapArray;    ///< Map representation used with plain pointer array.
+
+  float scaleToMap;              ///< Scaling factor from world to map.
+
+  Eigen::Affine2f worldTmap;     ///< Homogenous 2D transform from map to world coordinates.
+  Eigen::Affine3f worldTmap3D;   ///< Homogenous 3D transform from map to world coordinates.
+  Eigen::Affine2f mapTworld;     ///< Homogenous 2D transform from world to map coordinates.
+
+  MapDimensionProperties mapDimensionProperties;
+  int sizeX;
+
+private:
+  int lastUpdateIndex;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapCacheArray.h

@@ -0,0 +1,167 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMapCacheArray_h_
+#define __GridMapCacheArray_h_
+
+#include <Eigen/Core>
+
+class CachedMapElement
+{
+public:
+  float val;
+  int index;
+};
+
+/**
+ * Caches filtered grid map accesses in a two dimensional array of the same size as the map.
+ */
+class GridMapCacheArray
+{
+public:
+
+  /**
+   * Constructor
+   */
+  GridMapCacheArray()
+    : cacheArray(0)
+    , arrayDimensions(-1,-1)
+  {
+    currCacheIndex = 0;
+  }
+
+  /**
+   * Destructor
+   */
+  ~GridMapCacheArray()
+  {
+    deleteCacheArray();
+  }
+
+  /**
+   * Resets/deletes the cached data
+   */
+  void resetCache()
+  {
+    currCacheIndex++;
+  }
+
+  /**
+   * Checks wether cached data for coords is available. If this is the case, writes data into val.
+   * @param coords The coordinates
+   * @param val Reference to a float the data is written to if available
+   * @return Indicates if cached data is available
+   */
+  bool containsCachedData(int index, float& val)
+  {
+    const CachedMapElement& elem (cacheArray[index]);
+
+    if (elem.index == currCacheIndex) {
+      val = elem.val;
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  /**
+   * Caches float value val for coordinates coords.
+   * @param coords The coordinates
+   * @param val The value to be cached for coordinates.
+   */
+  void cacheData(int index, float val)
+  {
+    CachedMapElement& elem (cacheArray[index]);
+    elem.index = currCacheIndex;
+    elem.val = val;
+  }
+
+  /**
+   * Sets the map size and resizes the cache array accordingly
+   * @param sizeIn The map size.
+   */
+  void setMapSize(const Eigen::Vector2i& newDimensions)
+  {
+    setArraySize(newDimensions);
+  }
+
+protected:
+
+  /**
+   * Creates a cache array of size sizeIn.
+   * @param sizeIn The size of the array
+   */
+  void createCacheArray(const Eigen::Vector2i& newDimensions)
+  {
+    arrayDimensions = newDimensions;
+
+    int sizeX = arrayDimensions[0];
+    int sizeY = arrayDimensions[1];
+
+    int size = sizeX * sizeY;
+
+    cacheArray = new CachedMapElement [size];
+
+    for (int x = 0; x < size; ++x) {
+      cacheArray[x].index = -1;
+    }
+  }
+
+  /**
+   * Deletes the existing cache array.
+   */
+  void deleteCacheArray()
+  {
+    delete[] cacheArray;
+  }
+
+  /**
+   * Sets a new cache array size
+   */
+  void setArraySize(const Eigen::Vector2i& newDimensions)
+  {
+    if (this->arrayDimensions != newDimensions) {
+      if (cacheArray != 0) {
+        deleteCacheArray();
+        cacheArray = 0;
+      }
+      createCacheArray(newDimensions);
+    }
+  }
+
+protected:
+
+  CachedMapElement* cacheArray;    ///< Array used for caching data.
+  int currCacheIndex;              ///< The cache iteration index value
+
+  Eigen::Vector2i arrayDimensions; ///< The size of the array
+
+};
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapLogOdds.h

@@ -0,0 +1,212 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMapLogOdds_h_
+#define __GridMapLogOdds_h_
+
+#include <cmath>
+
+/**
+ * Provides a log odds of occupancy probability representation for cells in a occupancy grid map.
+ */
+class LogOddsCell
+{
+public:
+
+  /*
+  void setOccupied()
+  {
+    logOddsVal += 0.7f;
+  };
+
+  void setFree()
+  {
+    logOddsVal -= 0.4f;
+  };
+  */
+
+
+  /**
+   * Sets the cell value to val.
+   * @param val The log odds value.
+   */
+  void set(float val)
+  {
+    logOddsVal = val;
+  }
+
+  /**
+   * Returns the value of the cell.
+   * @return The log odds value.
+   */
+  float getValue() const
+  {
+    return logOddsVal;
+  }
+
+  /**
+   * Returns wether the cell is occupied.
+   * @return Cell is occupied
+   */
+  bool isOccupied() const
+  {
+    return logOddsVal > 0.0f;
+  }
+
+  bool isFree() const
+  {
+    return logOddsVal < 0.0f;
+  }
+
+  /**
+   * Reset Cell to prior probability.
+   */
+  void resetGridCell()
+  {
+    logOddsVal = 0.0f;
+    updateIndex = -1;
+  }
+
+  //protected:
+
+public:
+
+  float logOddsVal; ///< The log odds representation of occupancy probability.
+  int updateIndex;
+
+
+};
+
+/**
+ * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
+ */
+class GridMapLogOddsFunctions
+{
+public:
+
+  /**
+   * Constructor, sets parameters like free and occupied log odds ratios.
+   */
+  GridMapLogOddsFunctions()
+  {
+    this->setUpdateFreeFactor(0.4f);
+    this->setUpdateOccupiedFactor(0.6f);
+    /*
+    //float probOccupied = 0.6f;
+    float probOccupied = 0.9f;
+    float oddsOccupied = probOccupied / (1.0f - probOccupied);
+    logOddsOccupied = log(oddsOccupied);
+
+    float probFree = 0.4f;
+    float oddsFree = probFree / (1.0f - probFree);
+    logOddsFree = log(oddsFree);
+    */
+  }
+
+  /**
+   * Update cell as occupied
+   * @param cell The cell.
+   */
+  void updateSetOccupied(LogOddsCell& cell) const
+  {
+    if (cell.logOddsVal < 50.0f){
+      cell.logOddsVal += logOddsOccupied;
+    }
+  }
+
+  /**
+   * Update cell as free
+   * @param cell The cell.
+   */
+  void updateSetFree(LogOddsCell& cell) const
+  {
+
+    cell.logOddsVal += logOddsFree;
+
+  }
+
+  void updateUnsetFree(LogOddsCell& cell) const
+  {
+    cell.logOddsVal -= logOddsFree;
+  }
+
+  /**
+   * Get the probability value represented by the grid cell.
+   * @param cell The cell.
+   * @return The probability
+   */
+  float getGridProbability(const LogOddsCell& cell) const
+  {
+    float odds = exp(cell.logOddsVal);
+    return odds / (odds + 1.0f);
+
+    /*
+    float val = cell.logOddsVal;
+
+    //prevent #IND when doing exp(large number).
+    if (val > 50.0f) {
+      return 1.0f;
+    } else {
+      float odds = exp(val);
+      return odds / (odds + 1.0f);
+    }
+    */
+    //return 0.5f;
+  }
+
+  //void getGridValueMap( const LogOddsCell& cell) const{};
+  //void isOccupied(LogOddsCell& cell) {};
+
+  //void resetGridCell() {};
+
+  void setUpdateFreeFactor(float factor)
+  {
+    logOddsFree = probToLogOdds(factor);
+  }
+
+  void setUpdateOccupiedFactor(float factor)
+  {
+    logOddsOccupied = probToLogOdds(factor);
+  }
+
+protected:
+
+  float probToLogOdds(float prob)
+  {
+    float odds = prob / (1.0f - prob);
+    return log(odds);
+  }
+
+  float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied
+  float logOddsFree;     /// < The log odds representation of probability used for updating cells as free
+
+
+};
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapReflectanceCount.h

@@ -0,0 +1,111 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMapReflectanceCount_h_
+#define __GridMapReflectanceCount_h_
+
+/**
+ * Provides a reflectance count representation for cells in a occupancy grid map.
+ */
+class ReflectanceCell
+{
+public:
+
+  void set(float val)
+  {
+    probOccupied = val;
+  }
+
+  float getValue() const
+  {
+    return probOccupied;
+  }
+
+  bool isOccupied() const
+  {
+    return probOccupied > 0.5f;
+  }
+
+  bool isFree() const{
+    return probOccupied < 0.5f;
+  }
+
+  void resetGridCell()
+  {
+    probOccupied = 0.5f;
+    visitedCount = 0.0f;
+    reflectedCount = 0.0f;
+    updateIndex = -1;
+  }
+
+//protected:
+
+  float visitedCount;
+  float reflectedCount;
+  float probOccupied;
+  int updateIndex;
+};
+
+
+class GridMapReflectanceFunctions
+{
+public:
+
+  GridMapReflectanceFunctions()
+  {}
+
+  void updateSetOccupied(ReflectanceCell& cell) const
+  {
+    ++cell.reflectedCount;
+    ++cell.visitedCount;
+    cell.probOccupied = cell.reflectedCount / cell.visitedCount;
+  }
+
+  void updateSetFree(ReflectanceCell& cell) const
+  {
+    ++cell.visitedCount;
+    cell.probOccupied = cell.reflectedCount / cell.visitedCount;
+  }
+
+  void updateUnsetFree(ReflectanceCell& cell) const
+  {
+    --cell.visitedCount;
+    cell.probOccupied = cell.reflectedCount / cell.visitedCount;
+  }
+
+  float getGridProbability(const ReflectanceCell& cell) const
+  {
+    return cell.probOccupied;
+  }
+
+protected:
+
+};
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapSimpleCount.h

@@ -0,0 +1,159 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __GridMapSimpleCount_h_
+#define __GridMapSimpleCount_h_
+
+
+/**
+ * Provides a (very) simple count based representation of occupancy
+ */
+class SimpleCountCell
+{
+public:
+
+  /**
+   * Sets the cell value to val.
+   * @param val The log odds value.
+   */
+  void set(float val)
+  {
+    simpleOccVal = val;
+  }
+
+  /**
+   * Returns the value of the cell.
+   * @return The log odds value.
+   */
+  float getValue() const
+  {
+    return simpleOccVal;
+  }
+
+  /**
+   * Returns wether the cell is occupied.
+   * @return Cell is occupied
+   */
+  bool isOccupied() const
+  {
+    return (simpleOccVal > 0.5f);
+  }
+
+  bool isFree() const
+  {
+    return (simpleOccVal < 0.5f);
+  }
+
+  /**
+   * Reset Cell to prior probability.
+   */
+  void resetGridCell()
+  {
+    simpleOccVal = 0.5f;
+    updateIndex = -1;
+  }
+
+//protected:
+
+public:
+
+  float simpleOccVal; ///< The log odds representation of occupancy probability.
+  int updateIndex;
+
+
+};
+
+/**
+ * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
+ */
+class GridMapSimpleCountFunctions
+{
+public:
+
+  /**
+   * Constructor, sets parameters like free and occupied log odds ratios.
+   */
+  GridMapSimpleCountFunctions()
+  {
+    updateFreeVal = -0.10f;
+    updateOccVal  =  0.15f;
+
+    updateFreeLimit = -updateFreeVal + updateFreeVal/100.0f;
+    updateOccLimit  = 1.0f - (updateOccVal + updateOccVal/100.0f);
+  }
+
+  /**
+   * Update cell as occupied
+   * @param cell The cell.
+   */
+  void updateSetOccupied(SimpleCountCell& cell) const
+  {
+    if (cell.simpleOccVal < updateOccLimit){
+      cell.simpleOccVal += updateOccVal;
+    }
+  }
+
+  /**
+   * Update cell as free
+   * @param cell The cell.
+   */
+  void updateSetFree(SimpleCountCell& cell) const
+  {
+    if (cell.simpleOccVal > updateFreeLimit){
+      cell.simpleOccVal += updateFreeVal;
+    }
+  }
+
+  void updateUnsetFree(SimpleCountCell& cell) const
+  {
+    cell.simpleOccVal -= updateFreeVal;
+  }
+
+  /**
+   * Get the probability value represented by the grid cell.
+   * @param cell The cell.
+   * @return The probability
+   */
+  float getGridProbability(const SimpleCountCell& cell) const
+  {
+    return cell.simpleOccVal;
+  }
+
+protected:
+
+  float updateFreeVal;
+  float updateOccVal;
+
+  float updateFreeLimit;
+  float updateOccLimit;
+
+
+};
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/MapDimensionProperties.h

@@ -0,0 +1,100 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __MapDimensionProperties_h_
+#define __MapDimensionProperties_h_
+
+class MapDimensionProperties
+{
+public:
+  MapDimensionProperties()
+    : topLeftOffset(-1.0f,-1.0f)
+    , mapDimensions(-1,-1)
+    , cellLength(-1.0f)
+  {}
+
+
+  MapDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
+    : topLeftOffset(topLeftOffsetIn)
+    , mapDimensions(mapDimensionsIn)
+    , cellLength(cellLengthIn)
+  {
+    mapLimitsf = (mapDimensionsIn.cast<float>()).array() - 1.0f;
+  }
+
+  bool operator==(const MapDimensionProperties& other) const
+  {
+    return (topLeftOffset == other.topLeftOffset) && (mapDimensions == other.mapDimensions) && (cellLength == other.cellLength);
+  }
+
+  bool hasEqualDimensionProperties(const MapDimensionProperties& other) const
+  {
+    return (mapDimensions == other.mapDimensions);
+  }
+
+  bool hasEqualTransformationProperties(const MapDimensionProperties& other) const
+  {
+     return  (topLeftOffset == other.topLeftOffset) && (cellLength == other.cellLength);
+  }
+
+  bool pointOutOfMapBounds(const Eigen::Vector2f& coords) const
+  {
+    return ((coords[0] < 0.0f) || (coords[0] > mapLimitsf[0]) || (coords[1] < 0.0f) || (coords[1] > mapLimitsf[1]));
+  }
+
+  void setMapCellDims(const Eigen::Vector2i& newDims)
+  {
+    mapDimensions = newDims;
+    mapLimitsf = (newDims.cast<float>()).array() - 2.0f;
+  }
+
+  void setTopLeftOffset(const Eigen::Vector2f& topLeftOffsetIn)
+  {
+    topLeftOffset = topLeftOffsetIn;
+  }
+
+  void setSizeX(int sX) { mapDimensions[0] = sX; };
+  void setSizeY(int sY) { mapDimensions[1] = sY; };
+  void setCellLength(float cl) { cellLength = cl; };
+
+  const Eigen::Vector2f& getTopLeftOffset() const { return topLeftOffset; };
+  const Eigen::Vector2i& getMapDimensions() const { return mapDimensions; };
+  int getSizeX() const { return mapDimensions[0]; };
+  int getSizeY() const { return mapDimensions[1]; };
+  float getCellLength() const { return cellLength; };
+
+protected:
+  Eigen::Vector2f topLeftOffset;
+  Eigen::Vector2i mapDimensions;
+  Eigen::Vector2f mapLimitsf;
+  float cellLength;
+};
+
+#endif
+
+

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapBase.h

@@ -0,0 +1,273 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __OccGridMapBase_h_
+#define __OccGridMapBase_h_
+
+#include "GridMapBase.h"
+
+#include "../scan/DataPointContainer.h"
+#include "../util/UtilFunctions.h"
+
+#include <Eigen/Geometry>
+
+namespace hectorslam {
+
+template<typename ConcreteCellType, typename ConcreteGridFunctions>
+class OccGridMapBase
+  : public GridMapBase<ConcreteCellType>
+{
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  OccGridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
+    : GridMapBase<ConcreteCellType>(mapResolution, size, offset)
+    , currUpdateIndex(0)
+    , currMarkOccIndex(-1)
+    , currMarkFreeIndex(-1)
+  {}
+
+  virtual ~OccGridMapBase() {}
+
+  void updateSetOccupied(int index)
+  {
+    concreteGridFunctions.updateSetOccupied(this->getCell(index));
+  }
+
+  void updateSetFree(int index)
+  {
+    concreteGridFunctions.updateSetFree(this->getCell(index));
+  }
+
+  void updateUnsetFree(int index)
+  {
+    concreteGridFunctions.updateUnsetFree(this->getCell(index));
+  }
+
+  float getGridProbabilityMap(int index) const
+  {
+    return concreteGridFunctions.getGridProbability(this->getCell(index));
+  }
+
+  bool isOccupied(int xMap, int yMap) const
+  {
+    return (this->getCell(xMap,yMap).isOccupied());
+  }
+
+  bool isFree(int xMap, int yMap) const
+  {
+    return (this->getCell(xMap,yMap).isFree());
+  }
+
+  bool isOccupied(int index) const
+  {
+    return (this->getCell(index).isOccupied());
+  }
+
+  bool isFree(int index) const
+  {
+    return (this->getCell(index).isFree());
+  }
+
+  float getObstacleThreshold() const
+  {
+    ConcreteCellType temp;
+    temp.resetGridCell();
+    return concreteGridFunctions.getGridProbability(temp);
+  }
+
+  void setUpdateFreeFactor(float factor)
+  {
+    concreteGridFunctions.setUpdateFreeFactor(factor);
+  }
+
+  void setUpdateOccupiedFactor(float factor)
+  {
+    concreteGridFunctions.setUpdateOccupiedFactor(factor);
+  }
+
+  /**
+   * Updates the map using the given scan data and robot pose
+   * @param dataContainer Contains the laser scan data
+   * @param robotPoseWorld The 2D robot pose in world coordinates
+   */
+  void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
+  {
+    currMarkFreeIndex = currUpdateIndex + 1;
+    currMarkOccIndex = currUpdateIndex + 2;
+
+    //Get pose in map coordinates from pose in world coordinates
+    Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));
+
+    //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
+    Eigen::Affine2f poseTransform((Eigen::Translation2f(
+                                        mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));
+
+    //Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer)
+    Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());
+
+    //Get integer vector of laser beams start point
+    Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);
+
+    //Get number of valid beams in current scan
+    int numValidElems = dataContainer.getSize();
+
+    //std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";
+
+    //Iterate over all valid laser beams
+    for (int i = 0; i < numValidElems; ++i) {
+
+      //Get map coordinates of current beam endpoint
+      Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
+      //std::cout << "\ns\n" << scanEndMapf << "\n";
+
+      //add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)
+      scanEndMapf.array() += (0.5f);
+
+      //Get integer map coordinates of current beam endpoint
+      Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());
+
+      //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
+      if (scanBeginMapi != scanEndMapi){
+        updateLineBresenhami(scanBeginMapi, scanEndMapi);
+      }
+    }
+
+    //Tell the map that it has been updated
+    this->setUpdated();
+
+    //Increase update index (used for updating grid cells only once per incoming scan)
+    currUpdateIndex += 3;
+  }
+
+  inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){
+
+    int x0 = beginMap[0];
+    int y0 = beginMap[1];
+
+    //check if beam start point is inside map, cancel update if this is not the case
+    if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {
+      return;
+    }
+
+    int x1 = endMap[0];
+    int y1 = endMap[1];
+
+    //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";
+
+    //check if beam end point is inside map, cancel update if this is not the case
+    if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {
+      return;
+    }
+
+    int dx = x1 - x0;
+    int dy = y1 - y0;
+
+    unsigned int abs_dx = abs(dx);
+    unsigned int abs_dy = abs(dy);
+
+    int offset_dx = util::sign(dx);
+    int offset_dy = util::sign(dy) * this->sizeX;
+
+    unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();
+
+    //if x is dominant
+    if(abs_dx >= abs_dy){
+      int error_y = abs_dx / 2;
+      bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
+    }else{
+      //otherwise y is dominant
+      int error_x = abs_dy / 2;
+      bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
+    }
+
+    unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
+    this->bresenhamCellOcc(endOffset);
+
+  }
+
+  inline void bresenhamCellFree(unsigned int offset)
+  {
+    ConcreteCellType& cell (this->getCell(offset));
+
+    if (cell.updateIndex < currMarkFreeIndex) {
+      concreteGridFunctions.updateSetFree(cell);
+      cell.updateIndex = currMarkFreeIndex;
+    }
+  }
+
+  inline void bresenhamCellOcc(unsigned int offset)
+  {
+    ConcreteCellType& cell (this->getCell(offset));
+
+    if (cell.updateIndex < currMarkOccIndex) {
+
+      //if this cell has been updated as free in the current iteration, revert this
+      if (cell.updateIndex == currMarkFreeIndex) {
+        concreteGridFunctions.updateUnsetFree(cell);
+      }
+
+      concreteGridFunctions.updateSetOccupied(cell);
+      //std::cout << " setOcc " << "\n";
+      cell.updateIndex = currMarkOccIndex;
+    }
+  }
+
+  inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
+
+    this->bresenhamCellFree(offset);
+
+    unsigned int end = abs_da-1;
+
+    for(unsigned int i = 0; i < end; ++i){
+      offset += offset_a;
+      error_b += abs_db;
+
+      if((unsigned int)error_b >= abs_da){
+        offset += offset_b;
+        error_b -= abs_da;
+      }
+
+      this->bresenhamCellFree(offset);
+    }
+  }
+
+protected:
+
+  ConcreteGridFunctions concreteGridFunctions;
+  int currUpdateIndex;
+  int currMarkOccIndex;
+  int currMarkFreeIndex;
+};
+
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtil.h

@@ -0,0 +1,392 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __OccGridMapUtil_h_
+#define __OccGridMapUtil_h_
+
+#include <cmath>
+
+#include "../scan/DataPointContainer.h"
+#include "../util/UtilFunctions.h"
+
+namespace hectorslam {
+
+template<typename ConcreteOccGridMap, typename ConcreteCacheMethod>
+class OccGridMapUtil
+{
+public:
+
+  OccGridMapUtil(const ConcreteOccGridMap* gridMap)
+    : concreteGridMap(gridMap)
+    , size(0)
+  {
+    mapObstacleThreshold = gridMap->getObstacleThreshold();
+    cacheMethod.setMapSize(gridMap->getMapDimensions());
+  }
+
+  ~OccGridMapUtil()
+  {}
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const { return concreteGridMap->getWorldCoordsPose(mapPose); };
+  inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const { return concreteGridMap->getMapCoordsPose(worldPose); };
+
+  inline Eigen::Vector2f getWorldCoordsPoint(const Eigen::Vector2f& mapPoint) const { return concreteGridMap->getWorldCoords(mapPoint); };
+
+  void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)
+  {
+    int size = dataPoints.getSize();
+
+    Eigen::Affine2f transform(getTransformForState(pose));
+
+    float sinRot = sin(pose[2]);
+    float cosRot = cos(pose[2]);
+
+    H = Eigen::Matrix3f::Zero();
+    dTr = Eigen::Vector3f::Zero();
+
+    for (int i = 0; i < size; ++i) {
+
+      const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));
+
+      Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));
+
+      float funVal = 1.0f - transformedPointData[0];
+
+      dTr[0] += transformedPointData[1] * funVal;
+      dTr[1] += transformedPointData[2] * funVal;
+
+      float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);
+
+      dTr[2] += rotDeriv * funVal;
+
+      H(0, 0) += util::sqr(transformedPointData[1]);
+      H(1, 1) += util::sqr(transformedPointData[2]);
+      H(2, 2) += util::sqr(rotDeriv);
+
+      H(0, 1) += transformedPointData[1] * transformedPointData[2];
+      H(0, 2) += transformedPointData[1] * rotDeriv;
+      H(1, 2) += transformedPointData[2] * rotDeriv;
+    }
+
+    H(1, 0) = H(0, 1);
+    H(2, 0) = H(0, 2);
+    H(2, 1) = H(1, 2);
+
+  }
+
+  Eigen::Matrix3f getCovarianceForPose(const Eigen::Vector3f& mapPose, const DataContainer& dataPoints)
+  {
+
+    float deltaTransX = 1.5f;
+    float deltaTransY = 1.5f;
+    float deltaAng = 0.05f;
+
+    float x = mapPose[0];
+    float y = mapPose[1];
+    float ang = mapPose[2];
+
+    Eigen::Matrix<float, 3, 7> sigmaPoints;
+
+    sigmaPoints.block<3, 1>(0, 0) = Eigen::Vector3f(x + deltaTransX, y, ang);
+    sigmaPoints.block<3, 1>(0, 1) = Eigen::Vector3f(x - deltaTransX, y, ang);
+    sigmaPoints.block<3, 1>(0, 2) = Eigen::Vector3f(x, y + deltaTransY, ang);
+    sigmaPoints.block<3, 1>(0, 3) = Eigen::Vector3f(x, y - deltaTransY, ang);
+    sigmaPoints.block<3, 1>(0, 4) = Eigen::Vector3f(x, y, ang + deltaAng);
+    sigmaPoints.block<3, 1>(0, 5) = Eigen::Vector3f(x, y, ang - deltaAng);
+    sigmaPoints.block<3, 1>(0, 6) = mapPose;
+
+    Eigen::Matrix<float, 7, 1> likelihoods;
+
+    likelihoods[0] = getLikelihoodForState(Eigen::Vector3f(x + deltaTransX, y, ang), dataPoints);
+    likelihoods[1] = getLikelihoodForState(Eigen::Vector3f(x - deltaTransX, y, ang), dataPoints);
+    likelihoods[2] = getLikelihoodForState(Eigen::Vector3f(x, y + deltaTransY, ang), dataPoints);
+    likelihoods[3] = getLikelihoodForState(Eigen::Vector3f(x, y - deltaTransY, ang), dataPoints);
+    likelihoods[4] = getLikelihoodForState(Eigen::Vector3f(x, y, ang + deltaAng), dataPoints);
+    likelihoods[5] = getLikelihoodForState(Eigen::Vector3f(x, y, ang - deltaAng), dataPoints);
+    likelihoods[6] = getLikelihoodForState(Eigen::Vector3f(x, y, ang), dataPoints);
+
+    float invLhNormalizer = 1 / likelihoods.sum();
+
+    std::cout << "\n lhs:\n" << likelihoods;
+
+    Eigen::Vector3f mean(Eigen::Vector3f::Zero());
+
+    for (int i = 0; i < 7; ++i) {
+      mean += (sigmaPoints.block<3, 1>(0, i) * likelihoods[i]);
+    }
+
+    mean *= invLhNormalizer;
+
+    Eigen::Matrix3f covMatrixMap(Eigen::Matrix3f::Zero());
+
+    for (int i = 0; i < 7; ++i) {
+      Eigen::Vector3f sigPointMinusMean(sigmaPoints.block<3, 1>(0, i) - mean);
+      covMatrixMap += (likelihoods[i] * invLhNormalizer) * (sigPointMinusMean * (sigPointMinusMean.transpose()));
+    }
+
+    return covMatrixMap;
+
+    //covMatrix.cwise() * invLhNormalizer;
+    //transform = getTransformForState(Eigen::Vector3f(x-deltaTrans, y, ang);
+  }
+
+  Eigen::Matrix3f getCovMatrixWorldCoords(const Eigen::Matrix3f& covMatMap)
+  {
+
+    //std::cout << "\nCovMap:\n" << covMatMap;
+
+    Eigen::Matrix3f covMatWorld;
+
+    float scaleTrans = concreteGridMap->getCellLength();
+    float scaleTransSq = util::sqr(scaleTrans);
+
+    covMatWorld(0, 0) = covMatMap(0, 0) * scaleTransSq;
+    covMatWorld(1, 1) = covMatMap(1, 1) * scaleTransSq;
+
+    covMatWorld(1, 0) = covMatMap(1, 0) * scaleTransSq;
+    covMatWorld(0, 1) = covMatWorld(1, 0);
+
+    covMatWorld(2, 0) = covMatMap(2, 0) * scaleTrans;
+    covMatWorld(0, 2) = covMatWorld(2, 0);
+
+    covMatWorld(2, 1) = covMatMap(2, 1) * scaleTrans;
+    covMatWorld(1, 2) = covMatWorld(2, 1);
+
+    covMatWorld(2, 2) = covMatMap(2, 2);
+
+    return covMatWorld;
+  }
+
+  float getLikelihoodForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)
+  {
+    float resid = getResidualForState(state, dataPoints);
+
+    return getLikelihoodForResidual(resid, dataPoints.getSize());
+  }
+
+  float getLikelihoodForResidual(float residual, int numDataPoints)
+  {
+    float numDataPointsA = static_cast<int>(numDataPoints);
+    float sizef = static_cast<float>(numDataPointsA);
+
+    return 1 - (residual / sizef);
+  }
+
+  float getResidualForState(const Eigen::Vector3f& state, const DataContainer& dataPoints)
+  {
+    int size = dataPoints.getSize();
+
+    int stepSize = 1;
+    float residual = 0.0f;
+
+
+    Eigen::Affine2f transform(getTransformForState(state));
+
+    for (int i = 0; i < size; i += stepSize) {
+
+      float funval = 1.0f - interpMapValue(transform * dataPoints.getVecEntry(i));
+      residual += funval;
+    }
+
+    return residual;
+  }
+
+  float getUnfilteredGridPoint(Eigen::Vector2i& gridCoords) const
+  {
+    return (concreteGridMap->getGridProbabilityMap(gridCoords.x(), gridCoords.y()));
+  }
+
+  float getUnfilteredGridPoint(int index) const
+  {
+    return (concreteGridMap->getGridProbabilityMap(index));
+  }
+
+  float interpMapValue(const Eigen::Vector2f& coords)
+  {
+    //check if coords are within map limits.
+    if (concreteGridMap->pointOutOfMapBounds(coords)){
+      return 0.0f;
+    }
+
+    //map coords are alway positive, floor them by casting to int
+    Eigen::Vector2i indMin(coords.cast<int>());
+
+    //get factors for bilinear interpolation
+    Eigen::Vector2f factors(coords - indMin.cast<float>());
+
+    int sizeX = concreteGridMap->getSizeX();
+
+    int index = indMin[1] * sizeX + indMin[0];
+
+    // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
+    // filter gridPoint with gaussian and store in cache.
+    if (!cacheMethod.containsCachedData(index, intensities[0])) {
+      intensities[0] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[0]);
+    }
+
+    ++index;
+
+    if (!cacheMethod.containsCachedData(index, intensities[1])) {
+      intensities[1] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[1]);
+    }
+
+    index += sizeX-1;
+
+    if (!cacheMethod.containsCachedData(index, intensities[2])) {
+      intensities[2] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[2]);
+    }
+
+    ++index;
+
+    if (!cacheMethod.containsCachedData(index, intensities[3])) {
+      intensities[3] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[3]);
+    }
+
+    float xFacInv = (1.0f - factors[0]);
+    float yFacInv = (1.0f - factors[1]);
+
+    return
+      ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
+      ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1]));
+
+  }
+
+  Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords)
+  {
+    //check if coords are within map limits.
+    if (concreteGridMap->pointOutOfMapBounds(coords)){
+      return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+    }
+
+    //map coords are always positive, floor them by casting to int
+    Eigen::Vector2i indMin(coords.cast<int>());
+
+    //get factors for bilinear interpolation
+    Eigen::Vector2f factors(coords - indMin.cast<float>());
+
+    int sizeX = concreteGridMap->getSizeX();
+
+    int index = indMin[1] * sizeX + indMin[0];
+
+    // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained
+    // filter gridPoint with gaussian and store in cache.
+    if (!cacheMethod.containsCachedData(index, intensities[0])) {
+      intensities[0] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[0]);
+    }
+
+    ++index;
+
+    if (!cacheMethod.containsCachedData(index, intensities[1])) {
+      intensities[1] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[1]);
+    }
+
+    index += sizeX-1;
+
+    if (!cacheMethod.containsCachedData(index, intensities[2])) {
+      intensities[2] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[2]);
+    }
+
+    ++index;
+
+    if (!cacheMethod.containsCachedData(index, intensities[3])) {
+      intensities[3] = getUnfilteredGridPoint(index);
+      cacheMethod.cacheData(index, intensities[3]);
+    }
+
+    float dx1 = intensities[0] - intensities[1];
+    float dx2 = intensities[2] - intensities[3];
+
+    float dy1 = intensities[0] - intensities[2];
+    float dy2 = intensities[1] - intensities[3];
+
+    float xFacInv = (1.0f - factors[0]);
+    float yFacInv = (1.0f - factors[1]);
+
+    return Eigen::Vector3f(
+      ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
+      ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),
+      -((dx1 * xFacInv) + (dx2 * factors[0])),
+      -((dy1 * yFacInv) + (dy2 * factors[1]))
+    );
+  }
+
+  Eigen::Affine2f getTransformForState(const Eigen::Vector3f& transVector) const
+  {
+    return Eigen::Translation2f(transVector[0], transVector[1]) * Eigen::Rotation2Df(transVector[2]);
+  }
+
+  Eigen::Translation2f getTranslationForState(const Eigen::Vector3f& transVector) const
+  {
+    return Eigen::Translation2f(transVector[0], transVector[1]);
+  }
+
+  void resetCachedData()
+  {
+    cacheMethod.resetCache();
+  }
+
+  void resetSamplePoints()
+  {
+    samplePoints.clear();
+  }
+
+  const std::vector<Eigen::Vector3f>& getSamplePoints() const
+  {
+    return samplePoints;
+  }
+
+protected:
+
+  Eigen::Vector4f intensities;
+
+  ConcreteCacheMethod cacheMethod;
+
+  const ConcreteOccGridMap* concreteGridMap;
+
+  std::vector<Eigen::Vector3f> samplePoints;
+
+  int size;
+
+  float mapObstacleThreshold;
+};
+
+}
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtilConfig.h

@@ -0,0 +1,58 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __OccGridMapUtilConfig_h_
+#define __OccGridMapUtilConfig_h_
+
+#include "OccGridMapUtil.h"
+
+//#define SLAM_USE_HASH_CACHING
+#ifdef SLAM_USE_HASH_CACHING
+#include "GridMapCacheHash.h"
+typedef GridMapCacheHash GridMapCacheMethod;
+#else
+#include "GridMapCacheArray.h"
+typedef GridMapCacheArray GridMapCacheMethod;
+#endif
+
+namespace hectorslam {
+
+template<typename ConcreteOccGridMap>
+class OccGridMapUtilConfig
+  : public OccGridMapUtil<ConcreteOccGridMap, GridMapCacheMethod>
+{
+public:
+
+  OccGridMapUtilConfig(ConcreteOccGridMap* gridMap = 0)
+    : OccGridMapUtil<ConcreteOccGridMap, GridMapCacheMethod>(gridMap)
+  {}
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/matcher/ScanMatcher.h

@@ -0,0 +1,252 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _scanmatcher_h__
+#define _scanmatcher_h__
+
+#include <Eigen/Geometry>
+#include "../scan/DataPointContainer.h"
+#include "../util/UtilFunctions.h"
+
+#include "../util/DrawInterface.h"
+#include "../util/HectorDebugInfoInterface.h"
+
+namespace hectorslam{
+
+template<typename ConcreteOccGridMapUtil>
+class ScanMatcher
+{
+public:
+
+  ScanMatcher(DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0)
+    : drawInterface(drawInterfaceIn)
+    , debugInterface(debugInterfaceIn)
+  {}
+
+  ~ScanMatcher()
+  {}
+
+  Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations)
+  {
+    if (drawInterface){
+      drawInterface->setScale(0.05f);
+      drawInterface->setColor(0.0f,1.0f, 0.0f);
+      drawInterface->drawArrow(beginEstimateWorld);
+
+      Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
+
+      drawScan(beginEstimateMap, gridMapUtil, dataContainer);
+
+      drawInterface->setColor(1.0,0.0,0.0);
+    }
+
+    if (dataContainer.getSize() != 0) {
+
+      Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
+
+      Eigen::Vector3f estimate(beginEstimateMap);
+
+      estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
+      //bool notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
+
+      /*
+      const Eigen::Matrix2f& hessian (H.block<2,2>(0,0));
+
+
+      Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(hessian);
+
+      const Eigen::Vector2f& eigValues (eig.eigenvalues());
+
+      float cond = eigValues[1] / eigValues[0];
+      float determinant = (hessian.determinant());
+      */
+      //std::cout << "\n cond: " << cond << " det: " << determinant << "\n";
+
+
+      int numIter = maxIterations;
+
+
+      for (int i = 0; i < numIter; ++i) {
+        //std::cout << "\nest:\n" << estimate;
+
+        estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
+        //notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
+
+        if(drawInterface){
+          float invNumIterf = 1.0f/static_cast<float> (numIter);
+          drawInterface->setColor(static_cast<float>(i)*invNumIterf,0.0f, 0.0f);
+          drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate));
+          //drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast<float>(i)*0.05, 0.0f));
+        }
+
+        if(debugInterface){
+          debugInterface->addHessianMatrix(H);
+        }
+      }
+
+      if (drawInterface){
+        drawInterface->setColor(0.0,0.0,1.0);
+        drawScan(estimate, gridMapUtil, dataContainer);
+      }
+        /*
+        Eigen::Matrix2f testMat(Eigen::Matrix2f::Identity());
+        testMat(0,0) = 2.0f;
+
+        float angleWorldCoords = util::toRad(30.0f);
+        float sinAngle = sin(angleWorldCoords);
+        float cosAngle = cos(angleWorldCoords);
+
+        Eigen::Matrix2f rotMat;
+        rotMat << cosAngle, -sinAngle, sinAngle, cosAngle;
+        Eigen::Matrix2f covarianceRotated (rotMat * testMat * rotMat.transpose());
+
+        drawInterface->setColor(0.0,0.0,1.0,0.5);
+        drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covarianceRotated);
+        */
+
+
+
+        /*
+        Eigen::Matrix3f covMatMap (gridMapUtil.getCovarianceForPose(estimate, dataContainer));
+        std::cout << "\nestim:" << estimate;
+        std::cout << "\ncovMap\n" << covMatMap;
+        drawInterface->setColor(0.0,0.0,1.0,0.5);
+
+
+        Eigen::Matrix3f covMatWorld(gridMapUtil.getCovMatrixWorldCoords(covMatMap));
+         std::cout << "\ncovWorld\n" << covMatWorld;
+
+        drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatMap.block<2,2>(0,0));
+
+        drawInterface->setColor(1.0,0.0,0.0,0.5);
+        drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatWorld.block<2,2>(0,0));
+
+        std::cout << "\nH:\n" << H;
+
+        float determinant = H.determinant();
+        std::cout << "\nH_det: " << determinant;
+        */
+
+        /*
+        Eigen::Matrix2f covFromHessian(H.block<2,2>(0,0) * 1.0f);
+        //std::cout << "\nCovFromHess:\n" << covFromHessian;
+
+        drawInterface->setColor(0.0, 1.0, 0.0, 0.5);
+        drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),covFromHessian.inverse());
+
+        Eigen::Matrix3f covFromHessian3d(H * 1.0f);
+        //std::cout << "\nCovFromHess:\n" << covFromHessian;
+
+        drawInterface->setColor(1.0, 0.0, 0.0, 0.8);
+        drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),(covFromHessian3d.inverse()).block<2,2>(0,0));
+        */
+
+
+      estimate[2] = util::normalize_angle(estimate[2]);
+
+      covMatrix = Eigen::Matrix3f::Zero();
+      //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse());
+      //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0));
+
+
+      /*
+      covMatrix(0,0) = 1.0/(0.1*0.1);
+      covMatrix(1,1) = 1.0/(0.1*0.1);
+      covMatrix(2,2) = 1.0/((M_PI / 18.0f) * (M_PI / 18.0f));
+      */
+
+
+      covMatrix = H;
+
+      return gridMapUtil.getWorldCoordsPose(estimate);
+    }
+
+    return beginEstimateWorld;
+  }
+
+protected:
+
+  bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints)
+  {
+    gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);
+    //std::cout << "\nH\n" << H  << "\n";
+    //std::cout << "\ndTr\n" << dTr  << "\n";
+
+
+    if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) {
+
+
+      //H += Eigen::Matrix3f::Identity() * 1.0f;
+      Eigen::Vector3f searchDir (H.inverse() * dTr);
+
+      //std::cout << "\nsearchdir\n" << searchDir  << "\n";
+
+      if (searchDir[2] > 0.2f) {
+        searchDir[2] = 0.2f;
+        std::cout << "SearchDir angle change too large\n";
+      } else if (searchDir[2] < -0.2f) {
+        searchDir[2] = -0.2f;
+        std::cout << "SearchDir angle change too large\n";
+      }
+
+      updateEstimatedPose(estimate, searchDir);
+      return true;
+    }
+    return false;
+  }
+
+  void updateEstimatedPose(Eigen::Vector3f& estimate, const Eigen::Vector3f& change)
+  {
+    estimate += change;
+  }
+
+  void drawScan(const Eigen::Vector3f& pose, const ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer)
+  {
+    drawInterface->setScale(0.02);
+
+    Eigen::Affine2f transform(gridMapUtil.getTransformForState(pose));
+
+    int size = dataContainer.getSize();
+    for (int i = 0; i < size; ++i) {
+      const Eigen::Vector2f& currPoint (dataContainer.getVecEntry(i));
+      drawInterface->drawPoint(gridMapUtil.getWorldCoordsPoint(transform * currPoint));
+    }
+  }
+
+protected:
+  Eigen::Vector3f dTr;
+  Eigen::Matrix3f H;
+
+  DrawInterface* drawInterface;
+  HectorDebugInfoInterface* debugInterface;
+};
+
+}
+
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/scan/DataPointContainer.h

@@ -0,0 +1,100 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 __DataPointContainer_h_
+#define __DataPointContainer_h_
+
+#include <vector>
+
+namespace hectorslam {
+
+template<typename DataPointType>
+class DataPointContainer
+{
+public:
+
+  DataPointContainer(int size = 1000)
+  {
+    dataPoints.reserve(size);
+  }
+
+  void setFrom(const DataPointContainer& other, float factor)
+  {
+    origo = other.getOrigo()*factor;
+
+    dataPoints = other.dataPoints;
+
+    unsigned int size = dataPoints.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      dataPoints[i] *= factor;
+    }
+
+  }
+
+  void add(const DataPointType& dataPoint)
+  {
+    dataPoints.push_back(dataPoint);
+  }
+
+  void clear()
+  {
+    dataPoints.clear();
+  }
+
+  int getSize() const
+  {
+    return dataPoints.size();
+  }
+
+  const DataPointType& getVecEntry(int index) const
+  {
+    return dataPoints[index];
+  }
+
+  DataPointType getOrigo() const
+  {
+    return origo;
+  }
+
+  void setOrigo(const DataPointType& origoIn)
+  {
+    origo = origoIn;
+  }
+
+protected:
+
+  std::vector<DataPointType> dataPoints;
+  DataPointType origo;
+};
+
+typedef DataPointContainer<Eigen::Vector2f> DataContainer;
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/HectorSlamProcessor.h

@@ -0,0 +1,158 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _hectorslamprocessor_h__
+#define _hectorslamprocessor_h__
+
+#include "../map/GridMap.h"
+#include "../map/OccGridMapUtilConfig.h"
+#include "../matcher/ScanMatcher.h"
+#include "../scan/DataPointContainer.h"
+
+#include "../util/UtilFunctions.h"
+#include "../util/DrawInterface.h"
+#include "../util/HectorDebugInfoInterface.h"
+#include "../util/MapLockerInterface.h"
+
+#include "MapRepresentationInterface.h"
+#include "MapRepMultiMap.h"
+
+
+#include <float.h>
+
+namespace hectorslam{
+
+class HectorSlamProcessor
+{
+public:
+
+  HectorSlamProcessor(float mapResolution, int mapSizeX, int mapSizeY , const Eigen::Vector2f& startCoords, int multi_res_size, DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0)
+    : drawInterface(drawInterfaceIn)
+    , debugInterface(debugInterfaceIn)
+  {
+    mapRep = new MapRepMultiMap(mapResolution, mapSizeX, mapSizeY, multi_res_size, startCoords, drawInterfaceIn, debugInterfaceIn);
+
+    this->reset();
+
+    this->setMapUpdateMinDistDiff(0.4f *1.0f);
+    this->setMapUpdateMinAngleDiff(0.13f * 1.0f);
+  }
+
+  ~HectorSlamProcessor()
+  {
+    delete mapRep;
+  }
+
+  void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false)
+  {
+    //std::cout << "\nph:\n" << poseHintWorld << "\n";
+
+    Eigen::Vector3f newPoseEstimateWorld;
+
+    if (!map_without_matching){
+        newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
+    }else{
+        newPoseEstimateWorld = poseHintWorld;
+    }
+
+    lastScanMatchPose = newPoseEstimateWorld;
+
+    //std::cout << "\nt1:\n" << newPoseEstimateWorld << "\n";
+
+    //std::cout << "\n1";
+    //std::cout << "\n" << lastScanMatchPose << "\n";
+    if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching){
+
+      mapRep->updateByScan(dataContainer, newPoseEstimateWorld);
+
+      mapRep->onMapUpdated();
+      lastMapUpdatePose = newPoseEstimateWorld;
+    }
+
+    if(drawInterface){
+      const GridMap& gridMapRef (mapRep->getGridMap());
+      drawInterface->setColor(1.0, 0.0, 0.0);
+      drawInterface->setScale(0.15);
+
+      drawInterface->drawPoint(gridMapRef.getWorldCoords(Eigen::Vector2f::Zero()));
+      drawInterface->drawPoint(gridMapRef.getWorldCoords((gridMapRef.getMapDimensions().array()-1).cast<float>()));
+      drawInterface->drawPoint(Eigen::Vector2f(1.0f, 1.0f));
+
+      drawInterface->sendAndResetData();
+    }
+
+    if (debugInterface)
+    {
+      debugInterface->sendAndResetData();
+    }
+  }
+
+  void reset()
+  {
+    lastMapUpdatePose = Eigen::Vector3f(FLT_MAX, FLT_MAX, FLT_MAX);
+    lastScanMatchPose = Eigen::Vector3f::Zero();
+    //lastScanMatchPose.x() = -10.0f;
+    //lastScanMatchPose.y() = -15.0f;
+    //lastScanMatchPose.z() = M_PI*0.15f;
+
+    mapRep->reset();
+  }
+
+  const Eigen::Vector3f& getLastScanMatchPose() const { return lastScanMatchPose; };
+  const Eigen::Matrix3f& getLastScanMatchCovariance() const { return lastScanMatchCov; };
+  float getScaleToMap() const { return mapRep->getScaleToMap(); };
+
+  int getMapLevels() const { return mapRep->getMapLevels(); };
+  const GridMap& getGridMap(int mapLevel = 0) const { return mapRep->getGridMap(mapLevel); };
+
+  void addMapMutex(int i, MapLockerInterface* mapMutex) { mapRep->addMapMutex(i, mapMutex); };
+  MapLockerInterface* getMapMutex(int i) { return mapRep->getMapMutex(i); };
+
+  void setUpdateFactorFree(float free_factor) { mapRep->setUpdateFactorFree(free_factor); };
+  void setUpdateFactorOccupied(float occupied_factor) { mapRep->setUpdateFactorOccupied(occupied_factor); };
+  void setMapUpdateMinDistDiff(float minDist) { paramMinDistanceDiffForMapUpdate = minDist; };
+  void setMapUpdateMinAngleDiff(float angleChange) { paramMinAngleDiffForMapUpdate = angleChange; };
+
+protected:
+
+  MapRepresentationInterface* mapRep;
+
+  Eigen::Vector3f lastMapUpdatePose;
+  Eigen::Vector3f lastScanMatchPose;
+  Eigen::Matrix3f lastScanMatchCov;
+
+  float paramMinDistanceDiffForMapUpdate;
+  float paramMinAngleDiffForMapUpdate;
+
+  DrawInterface* drawInterface;
+  HectorDebugInfoInterface* debugInterface;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapProcContainer.h

@@ -0,0 +1,126 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _hectormapproccontainer_h__
+#define _hectormapproccontainer_h__
+
+#include "../map/GridMap.h"
+#include "../map/OccGridMapUtilConfig.h"
+#include "../matcher/ScanMatcher.h"
+#include "../util/MapLockerInterface.h"
+
+class GridMap;
+class ConcreteOccGridMapUtil;
+class DataContainer;
+
+namespace hectorslam{
+
+class MapProcContainer
+{
+public:
+  MapProcContainer(GridMap* gridMapIn, OccGridMapUtilConfig<GridMap>* gridMapUtilIn, ScanMatcher<OccGridMapUtilConfig<GridMap> >* scanMatcherIn)
+    : gridMap(gridMapIn)
+    , gridMapUtil(gridMapUtilIn)
+    , scanMatcher(scanMatcherIn)
+    , mapMutex(0)
+  {}
+
+  virtual ~MapProcContainer()
+  {}
+
+  void cleanup()
+  {
+    delete gridMap;
+    delete gridMapUtil;
+    delete scanMatcher;
+
+    if (mapMutex){
+      delete mapMutex;
+    }
+  }
+
+  void reset()
+  {
+    gridMap->reset();
+    gridMapUtil->resetCachedData();
+  }
+
+  void resetCachedData()
+  {
+    gridMapUtil->resetCachedData();
+  }
+
+  float getScaleToMap() const { return gridMap->getScaleToMap(); };
+
+  const GridMap& getGridMap() const { return *gridMap; };
+  GridMap& getGridMap() { return *gridMap; };
+
+  void addMapMutex(MapLockerInterface* mapMutexIn)
+  {
+    if (mapMutex)
+    {
+      delete mapMutex;
+    }
+
+    mapMutex = mapMutexIn;
+  }
+
+  MapLockerInterface* getMapMutex()
+  {
+    return mapMutex;
+  }
+
+  Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations)
+  {
+    return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations);
+  }
+
+  void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
+  {
+    if (mapMutex)
+    {
+      mapMutex->lockMap();
+    }
+
+    gridMap->updateByScan(dataContainer, robotPoseWorld);
+
+    if (mapMutex)
+    {
+      mapMutex->unlockMap();
+    }
+  }
+
+  GridMap* gridMap;
+  OccGridMapUtilConfig<GridMap>* gridMapUtil;
+  ScanMatcher<OccGridMapUtilConfig<GridMap> >* scanMatcher;
+  MapLockerInterface* mapMutex;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepMultiMap.h

@@ -0,0 +1,176 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _hectormaprepmultimap_h__
+#define _hectormaprepmultimap_h__
+
+#include "MapRepresentationInterface.h"
+#include "MapProcContainer.h"
+
+#include "../map/GridMap.h"
+#include "../map/OccGridMapUtilConfig.h"
+#include "../matcher/ScanMatcher.h"
+
+#include "../util/DrawInterface.h"
+#include "../util/HectorDebugInfoInterface.h"
+
+namespace hectorslam{
+
+class MapRepMultiMap : public MapRepresentationInterface
+{
+
+public:
+  MapRepMultiMap(float mapResolution, int mapSizeX, int mapSizeY, unsigned int numDepth, const Eigen::Vector2f& startCoords, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn)
+  {
+    //unsigned int numDepth = 3;
+    Eigen::Vector2i resolution(mapSizeX, mapSizeY);
+
+    float totalMapSizeX = mapResolution * static_cast<float>(mapSizeX);
+    float mid_offset_x = totalMapSizeX * startCoords.x();
+
+    float totalMapSizeY = mapResolution * static_cast<float>(mapSizeY);
+    float mid_offset_y = totalMapSizeY * startCoords.y();
+
+    for (unsigned int i = 0; i < numDepth; ++i){
+      std::cout << "HectorSM map lvl " << i << ": cellLength: " << mapResolution << " res x:" << resolution.x() << " res y: " << resolution.y() << "\n";
+      GridMap* gridMap = new hectorslam::GridMap(mapResolution,resolution, Eigen::Vector2f(mid_offset_x, mid_offset_y));
+      OccGridMapUtilConfig<GridMap>* gridMapUtil = new OccGridMapUtilConfig<GridMap>(gridMap);
+      ScanMatcher<OccGridMapUtilConfig<GridMap> >* scanMatcher = new hectorslam::ScanMatcher<OccGridMapUtilConfig<GridMap> >(drawInterfaceIn, debugInterfaceIn);
+
+      mapContainer.push_back(MapProcContainer(gridMap, gridMapUtil, scanMatcher));
+
+      resolution /= 2;
+      mapResolution*=2.0f;
+    }
+
+    dataContainers.resize(numDepth-1);
+  }
+
+  virtual ~MapRepMultiMap()
+  {
+    unsigned int size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      mapContainer[i].cleanup();
+    }
+  }
+
+  virtual void reset()
+  {
+    unsigned int size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      mapContainer[i].reset();
+    }
+  }
+
+  virtual float getScaleToMap() const { return mapContainer[0].getScaleToMap(); };
+
+  virtual int getMapLevels() const { return mapContainer.size(); };
+  virtual const GridMap& getGridMap(int mapLevel) const { return mapContainer[mapLevel].getGridMap(); };
+
+  virtual void addMapMutex(int i, MapLockerInterface* mapMutex)
+  {
+    mapContainer[i].addMapMutex(mapMutex);
+  }
+
+  MapLockerInterface* getMapMutex(int i)
+  {
+    return mapContainer[i].getMapMutex();
+  }
+
+  virtual void onMapUpdated()
+  {
+    unsigned int size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      mapContainer[i].resetCachedData();
+    }
+  }
+
+  virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix)
+  {
+    size_t size = mapContainer.size();
+
+    Eigen::Vector3f tmp(beginEstimateWorld);
+
+    for (int index = size - 1; index >= 0; --index){
+      //std::cout << " m " << i;
+      if (index == 0){
+        tmp  = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5));
+      }else{
+        dataContainers[index-1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
+        tmp  = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3));
+      }
+    }
+    return tmp;
+  }
+
+  virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
+  {
+    unsigned int size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      //std::cout << " u " <<  i;
+      if (i==0){
+        mapContainer[i].updateByScan(dataContainer, robotPoseWorld);
+      }else{
+        mapContainer[i].updateByScan(dataContainers[i-1], robotPoseWorld);
+      }
+    }
+    //std::cout << "\n";
+  }
+
+  virtual void setUpdateFactorFree(float free_factor)
+  {
+    size_t size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      GridMap& map = mapContainer[i].getGridMap();
+      map.setUpdateFreeFactor(free_factor);
+    }
+  }
+
+  virtual void setUpdateFactorOccupied(float occupied_factor)
+  {
+    size_t size = mapContainer.size();
+
+    for (unsigned int i = 0; i < size; ++i){
+      GridMap& map = mapContainer[i].getGridMap();
+      map.setUpdateOccupiedFactor(occupied_factor);
+    }
+  }
+
+protected:
+  std::vector<MapProcContainer> mapContainer;
+  std::vector<DataContainer> dataContainers;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepSingleMap.h

@@ -0,0 +1,97 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _hectormaprepsinglemap_h__
+#define _hectormaprepsinglemap_h__
+
+#include "MapRepresentationInterface.h"
+
+#include "../map/GridMap.h"
+#include "../map/OccGridMapUtilConfig.h"
+#include "../matcher/ScanMatcher.h"
+
+#include "../util/DrawInterface.h"
+#include "../util/HectorDebugInfoInterface.h"
+
+namespace hectorslam{
+
+class MapRepSingleMap : public MapRepresentationInterface
+{
+
+public:
+  MapRepSingleMap(float mapResolution, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn)
+  {
+    gridMap = new hectorslam::GridMap(mapResolution,Eigen::Vector2i(1024,1024), Eigen::Vector2f(20.0f, 20.0f));
+    gridMapUtil = new OccGridMapUtilConfig<GridMap>(gridMap);
+    scanMatcher = new hectorslam::ScanMatcher<OccGridMapUtilConfig<GridMap> >(drawInterfaceIn, debugInterfaceIn);
+  }
+
+  virtual ~MapRepSingleMap()
+  {
+    delete gridMap;
+    delete gridMapUtil;
+    delete scanMatcher;
+  }
+
+  virtual void reset()
+  {
+    gridMap->reset();
+    gridMapUtil->resetCachedData();
+  }
+
+  virtual float getScaleToMap() const { return gridMap->getScaleToMap(); };
+
+  virtual int getMapLevels() const { return 1; };
+  virtual const GridMap& getGridMap(int mapLevel) const { return *gridMap; };
+
+  virtual void onMapUpdated()
+  {
+    gridMapUtil->resetCachedData();
+  }
+
+  virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix)
+  {
+    return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, 20);
+  }
+
+  virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
+  {
+    gridMap->updateByScan(dataContainer, robotPoseWorld);
+  }
+
+protected:
+  GridMap* gridMap;
+  OccGridMapUtilConfig<GridMap>* gridMapUtil;
+  ScanMatcher<OccGridMapUtilConfig<GridMap> >* scanMatcher;
+};
+
+}
+
+#endif
+
+

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepresentationInterface.h

@@ -0,0 +1,66 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 _hectormaprepresentationinterface_h__
+#define _hectormaprepresentationinterface_h__
+
+class GridMap;
+class ConcreteOccGridMapUtil;
+class DataContainer;
+
+namespace hectorslam{
+
+class MapRepresentationInterface
+{
+public:
+
+  virtual ~MapRepresentationInterface() {};
+
+  virtual void reset() = 0;
+
+  virtual float getScaleToMap() const = 0;
+
+  virtual int getMapLevels() const = 0;
+  virtual const GridMap& getGridMap(int mapLevel = 0) const = 0;
+
+  virtual void addMapMutex(int i, MapLockerInterface* mapMutex) = 0;
+  virtual MapLockerInterface* getMapMutex(int i) = 0;
+
+  virtual void onMapUpdated() = 0;
+
+  virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) = 0;
+
+  virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) = 0;
+
+  virtual void setUpdateFactorFree(float free_factor) = 0;
+  virtual void setUpdateFactorOccupied(float occupied_factor) = 0;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/DrawInterface.h

@@ -0,0 +1,44 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 drawinterface_h__
+#define drawinterface_h__
+
+class DrawInterface{
+public:
+  virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0;
+  virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0;
+  virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0;
+
+  virtual void setScale(double scale) = 0;
+  virtual void setColor(double r, double g, double b, double a = 1.0) = 0;
+
+  virtual void sendAndResetData() = 0;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/HectorDebugInfoInterface.h

@@ -0,0 +1,40 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 hectordebuginfointerface_h__
+#define hectordebuginfointerface_h__
+
+class HectorDebugInfoInterface{
+public:
+
+  virtual void sendAndResetData() = 0;
+  virtual void addHessianMatrix(const Eigen::Matrix3f& hessian) = 0;
+  virtual void addPoseLikelihood(float lh) = 0;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/MapLockerInterface.h

@@ -0,0 +1,39 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 maplockerinterface_h__
+#define maplockerinterface_h__
+
+class MapLockerInterface
+{
+public:
+  virtual void lockMap() = 0;
+  virtual void unlockMap() = 0;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/UtilFunctions.h

@@ -0,0 +1,101 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 utilfunctions_h__
+#define utilfunctions_h__
+
+#include <cmath>
+#include <tf/LinearMath/Transform.h>
+
+namespace util{
+
+static inline float normalize_angle_pos(float angle)
+{
+  return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI);
+}
+
+static inline float normalize_angle(float angle)
+{
+  float a = normalize_angle_pos(angle);
+  if (a > M_PI){
+    a -= 2.0f*M_PI;
+  }
+  return a;
+}
+
+static inline float sqr(float val)
+{
+  return val*val;
+}
+
+static inline int sign(int x)
+{
+  return x > 0 ? 1 : -1;
+}
+
+template<typename T>
+static T toDeg(const T radVal)
+{
+  return radVal * static_cast<T>(180.0 / M_PI);
+}
+
+template<typename T>
+static T toRad(const T degVal)
+{
+  return degVal * static_cast<T>(M_PI / 180.0);
+}
+
+static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1, const Eigen::Vector3f& pose2, float distanceDiffThresh, float angleDiffThresh)
+{
+  //check distance
+  if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){
+    return true;
+  }
+
+  float angleDiff = (pose1.z() - pose2.z());
+
+  if (angleDiff > M_PI) {
+    angleDiff -= M_PI * 2.0f;
+  } else if (angleDiff < -M_PI) {
+    angleDiff += M_PI * 2.0f;
+  }
+
+  if (abs(angleDiff) > angleDiffThresh){
+    return true;
+  }
+  return false;
+}
+
+static double getYawFromQuat(const geometry_msgs::Quaternion &quat)
+{
+  return tf::getYaw(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
+}
+
+}
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_mapping/launch/mapping_default.launch

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
+  <arg name="base_frame" default="base_footprint"/>
+  <arg name="odom_frame" default="nav"/>
+  <arg name="pub_map_odom_transform" default="true"/>
+  <arg name="scan_subscriber_queue_size" default="5"/>
+  <arg name="scan_topic" default="scan"/>
+  <arg name="map_size" default="2048"/>
+  
+  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+    
+    <!-- Frame names -->
+    <param name="map_frame" value="map" />
+    <param name="base_frame" value="$(arg base_frame)" />
+    <param name="odom_frame" value="$(arg odom_frame)" />
+    
+    <!-- Tf use -->
+    <param name="use_tf_scan_transformation" value="true"/>
+    <param name="use_tf_pose_start_estimate" value="false"/>
+    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
+    
+    <!-- Map size / start point -->
+    <param name="map_resolution" value="0.050"/>
+    <param name="map_size" value="$(arg map_size)"/>
+    <param name="map_start_x" value="0.5"/>
+    <param name="map_start_y" value="0.5" />
+    <param name="map_multi_res_levels" value="2" />
+    
+    <!-- Map update parameters -->
+    <param name="update_factor_free" value="0.4"/>
+    <param name="update_factor_occupied" value="0.9" />    
+    <param name="map_update_distance_thresh" value="0.4"/>
+    <param name="map_update_angle_thresh" value="0.06" />
+    <param name="laser_z_min_value" value = "-1.0" />
+    <param name="laser_z_max_value" value = "1.0" />
+    
+    <!-- Advertising config --> 
+    <param name="advertise_map_service" value="true"/>
+    
+    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
+    <param name="scan_topic" value="$(arg scan_topic)"/>
+    
+    <!-- Debug parameters -->
+    <!--
+      <param name="output_timing" value="false"/>
+      <param name="pub_drawings" value="true"/>
+      <param name="pub_debug_output" value="true"/>
+    -->
+    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
+  </node>
+    
+  <!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
+</launch>
+  
+  

Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorDebugInfo.msg

@@ -0,0 +1 @@
+HectorIterData[] iterData

Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorIterData.msg

@@ -0,0 +1,5 @@
+float64[9] hessian
+float64 conditionNum
+float64 determinant
+float64 conditionNum2d
+float64 determinant2d

Localizations/Libraries/Ros/hector_slam/hector_mapping/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_mapping</name>
+  <version>0.5.2</version>
+  <description>
+    hector_mapping is a SLAM approach that can be used without odometry as well as on platforms that exhibit roll/pitch motion (of the sensor, the platform or both).
+    It leverages the high update rate of modern LIDAR systems like the Hokuyo UTM-30LX and provides 2D pose estimates at scan rate of the sensors (40Hz for the UTM-30LX).
+    While the system does not provide explicit loop closing ability, it is sufficiently accurate for many real world scenarios. The system has successfully been used on
+    Unmanned Ground Robots, Unmanned Surface Vehicles, Handheld Mapping Devices and logged data from quadrotor UAVs.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_mapping</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>laser_geometry</build_depend>
+  <build_depend>eigen</build_depend>
+  <build_depend>boost</build_depend>
+  <build_depend>message_generation</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>laser_geometry</run_depend>
+  <run_depend>eigen</run_depend>
+  <run_depend>boost</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_mapping/src/HectorMappingRos.cpp

@@ -0,0 +1,627 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_mapping/HectorMappingRos.h"
+
+#include "hector_mapping/map/GridMap.h"
+
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/Odometry.h>
+
+#include "sensor_msgs/PointCloud2.h"
+
+#include "hector_mapping/HectorDrawings.h"
+#include "hector_mapping/HectorDebugInfoProvider.h"
+#include "hector_mapping/HectorMapMutex.h"
+
+#ifndef TF_SCALAR_H
+  typedef btScalar tfScalar;
+#endif
+
+HectorMappingRos::HectorMappingRos()
+  : debugInfoProvider(0)
+  , hectorDrawings(0)
+  , lastGetMapUpdateIndex(-100)
+  , tfB_(0)
+  , map__publish_thread_(0)
+  , initial_pose_set_(false)
+  , pause_scan_processing_(false)
+{
+  ros::NodeHandle private_nh_("~");
+
+  std::string mapTopic_ = "map";
+
+  private_nh_.param("pub_drawings", p_pub_drawings, false);
+  private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
+  private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
+  private_nh_.param("pub_odometry", p_pub_odometry_,false);
+  private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
+  private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
+
+  private_nh_.param("map_resolution", p_map_resolution_, 0.025);
+  private_nh_.param("map_size", p_map_size_, 1024);
+  private_nh_.param("map_start_x", p_map_start_x_, 0.5);
+  private_nh_.param("map_start_y", p_map_start_y_, 0.5);
+  private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
+
+  private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
+  private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
+
+  private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
+  private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
+
+  private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
+  private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
+  private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
+
+  private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
+  private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
+  private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
+
+  private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
+  private_nh_.param("map_frame", p_map_frame_, std::string("map"));
+  private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
+
+  private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
+  private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame"));
+
+  private_nh_.param("output_timing", p_timing_output_,false);
+
+  private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
+
+  double tmp = 0.0;
+  private_nh_.param("laser_min_dist", tmp, 0.4);
+  p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
+
+  private_nh_.param("laser_max_dist", tmp, 30.0);
+  p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
+
+  private_nh_.param("laser_z_min_value", tmp, -1.0);
+  p_laser_z_min_value_ = static_cast<float>(tmp);
+
+  private_nh_.param("laser_z_max_value", tmp, 1.0);
+  p_laser_z_max_value_ = static_cast<float>(tmp);
+
+  if (p_pub_drawings)
+  {
+    ROS_INFO("HectorSM publishing debug drawings");
+    hectorDrawings = new HectorDrawings();
+  }
+
+  if(p_pub_debug_output_)
+  {
+    ROS_INFO("HectorSM publishing debug info");
+    debugInfoProvider = new HectorDebugInfoProvider();
+  }
+
+  if(p_pub_odometry_)
+  {
+    odometryPublisher_ = node_.advertise<nav_msgs::Odometry>("scanmatch_odom", 50);
+  }
+
+  slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider);
+  slamProcessor->setUpdateFactorFree(p_update_factor_free_);
+  slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);
+  slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);
+  slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);
+
+  int mapLevels = slamProcessor->getMapLevels();
+  mapLevels = 1;
+
+  for (int i = 0; i < mapLevels; ++i)
+  {
+    mapPubContainer.push_back(MapPublisherContainer());
+    slamProcessor->addMapMutex(i, new HectorMapMutex());
+
+    std::string mapTopicStr(mapTopic_);
+
+    if (i != 0)
+    {
+      mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
+    }
+
+    std::string mapMetaTopicStr(mapTopicStr);
+    mapMetaTopicStr.append("_metadata");
+
+    MapPublisherContainer& tmp = mapPubContainer[i];
+    tmp.mapPublisher_ = node_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
+    tmp.mapMetadataPublisher_ = node_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);
+
+    if ( (i == 0) && p_advertise_map_service_)
+    {
+      tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this);
+    }
+
+    setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i));
+
+    if ( i== 0){
+      mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
+    }
+  }
+
+  // Initialize services
+  reset_map_service_ = node_.advertiseService("reset_map", &HectorMappingRos::resetMapCallback, this);
+  restart_hector_service_ = node_.advertiseService("restart_mapping_with_new_pose", &HectorMappingRos::restartHectorCallback, this);
+  toggle_scan_processing_service_ = node_.advertiseService("pause_mapping", &HectorMappingRos::pauseMapCallback, this);
+
+  ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str());
+  ROS_INFO("HectorSM p_map_frame_: %s", p_map_frame_.c_str());
+  ROS_INFO("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str());
+  ROS_INFO("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str());
+  ROS_INFO("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false"));
+  ROS_INFO("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false"));
+  ROS_INFO("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_);
+  ROS_INFO("HectorSM p_map_pub_period_: %f", p_map_pub_period_);
+  ROS_INFO("HectorSM p_update_factor_free_: %f", p_update_factor_free_);
+  ROS_INFO("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_);
+  ROS_INFO("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_);
+  ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_);
+  ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_);
+  ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_);
+
+  scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);
+  sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);
+
+  poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1, false);
+  posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);
+
+  scan_point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud>("slam_cloud",1,false);
+
+  tfB_ = new tf::TransformBroadcaster();
+  ROS_ASSERT(tfB_);
+
+  /*
+  bool p_use_static_map_ = false;
+
+  if (p_use_static_map_){
+    mapSubscriber_ = node_.subscribe(mapTopic_, 1, &HectorMappingRos::staticMapCallback, this);
+  }
+  */
+
+  initial_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(node_, "initialpose", 2);
+  initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, tf_, p_map_frame_, 2);
+  initial_pose_filter_->registerCallback(boost::bind(&HectorMappingRos::initialPoseCallback, this, _1));
+
+
+  map__publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_));
+
+  map_to_odom_.setIdentity();
+
+  lastMapPublishTime = ros::Time(0,0);
+}
+
+HectorMappingRos::~HectorMappingRos()
+{
+  delete slamProcessor;
+
+  if (hectorDrawings)
+    delete hectorDrawings;
+
+  if (debugInfoProvider)
+    delete debugInfoProvider;
+
+  if (tfB_)
+    delete tfB_;
+
+  if(map__publish_thread_)
+    delete map__publish_thread_;
+}
+
+void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
+{
+  if (pause_scan_processing_)
+  {
+    return;
+  }
+
+  if (hectorDrawings)
+  {
+    hectorDrawings->setTime(scan.header.stamp);
+  }
+
+  ros::WallTime start_time = ros::WallTime::now();
+  if (!p_use_tf_scan_transformation_)
+  {
+    // If we are not using the tf tree to find the transform between the base frame and laser frame,
+    // then just convert the laser scan to our data container and process the update based on our last
+    // pose estimate
+    this->rosLaserScanToDataContainer(scan, laserScanContainer, slamProcessor->getScaleToMap());
+    slamProcessor->update(laserScanContainer, slamProcessor->getLastScanMatchPose());
+  }
+  else
+  {
+    // If we are using the tf tree to find the transform between the base frame and laser frame,
+    // let's get that transform
+    const ros::Duration dur(0.5);
+    tf::StampedTransform laser_transform;
+    if (tf_.waitForTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, dur))
+    {
+      tf_.lookupTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, laser_transform);
+    }
+    else
+    {
+      ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
+      return;
+    }
+
+    // Convert the laser scan to point cloud
+    projector_.projectLaser(scan, laser_point_cloud_, 30.0);
+
+    // Publish the point cloud if there are any subscribers
+    if (scan_point_cloud_publisher_.getNumSubscribers() > 0)
+    {
+      scan_point_cloud_publisher_.publish(laser_point_cloud_);
+    }
+
+    // Convert the point cloud to our data container
+    this->rosPointCloudToDataContainer(laser_point_cloud_, laser_transform, laserScanContainer, slamProcessor->getScaleToMap());
+
+    // Now let's choose the initial pose estimate for our slam process update
+    Eigen::Vector3f start_estimate(Eigen::Vector3f::Zero());
+    if (initial_pose_set_)
+    {
+      // User has requested a pose reset
+      initial_pose_set_ = false;
+      start_estimate = initial_pose_;
+    }
+    else if (p_use_tf_pose_start_estimate_)
+    {
+      // Initial pose estimate comes from the tf tree
+      try
+      {
+        tf::StampedTransform stamped_pose;
+
+        tf_.waitForTransform(p_map_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
+        tf_.lookupTransform(p_map_frame_, p_base_frame_,  scan.header.stamp, stamped_pose);
+
+        const double yaw = tf::getYaw(stamped_pose.getRotation());
+        start_estimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(), stamped_pose.getOrigin().getY(), yaw);
+      }
+      catch(tf::TransformException e)
+      {
+        ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
+        start_estimate = slamProcessor->getLastScanMatchPose();
+      }
+    }
+    else
+    {
+      // If none of the above, the initial pose is simply the last estimated pose
+      start_estimate = slamProcessor->getLastScanMatchPose();
+    }
+
+    // If "p_map_with_known_poses_" is enabled, we assume that start_estimate is precise and doesn't need to be refined
+    if (p_map_with_known_poses_)
+    {
+      slamProcessor->update(laserScanContainer, start_estimate, true);
+    }
+    else
+    {
+      slamProcessor->update(laserScanContainer, start_estimate);
+    }
+  }
+
+  // If the debug flag "p_timing_output_" is enabled, print how long this last iteration took
+  if (p_timing_output_)
+  {
+    ros::WallDuration duration = ros::WallTime::now() - start_time;
+    ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
+  }
+
+  // If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
+  if (p_map_with_known_poses_)
+  {
+    return;
+  }
+
+  poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);
+
+  // Publish pose with and without covariances
+  poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
+  posePublisher_.publish(poseInfoContainer_.getPoseStamped());
+
+  // Publish odometry if enabled
+  if(p_pub_odometry_)
+  {
+    nav_msgs::Odometry tmp;
+    tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
+
+    tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
+    tmp.child_frame_id = p_base_frame_;
+    odometryPublisher_.publish(tmp);
+  }
+
+  // Publish the map->odom transform if enabled
+  if (p_pub_map_odom_transform_)
+  {
+    tf::StampedTransform odom_to_base;
+    try
+    {
+      tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
+      tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);
+    }
+    catch(tf::TransformException e)
+    {
+      ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what());
+      odom_to_base.setIdentity();
+    }
+    map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
+    tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
+  }
+
+  // Publish the transform from map to estimated pose (if enabled)
+  if (p_pub_map_scanmatch_transform_)
+  {
+    tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
+  }
+}
+
+void HectorMappingRos::sysMsgCallback(const std_msgs::String& string)
+{
+  ROS_INFO("HectorSM sysMsgCallback, msg contents: %s", string.data.c_str());
+
+  if (string.data == "reset")
+  {
+    ROS_INFO("HectorSM reset");
+    slamProcessor->reset();
+  }
+}
+
+bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request  &req,
+                                   nav_msgs::GetMap::Response &res)
+{
+  ROS_INFO("HectorSM Map service called");
+  res = mapPubContainer[0].map_;
+  return true;
+}
+
+bool HectorMappingRos::resetMapCallback(std_srvs::Trigger::Request  &req,
+                                        std_srvs::Trigger::Response &res)
+{
+  ROS_INFO("HectorSM Reset map service called");
+  slamProcessor->reset();
+  return true;
+}
+
+bool HectorMappingRos::restartHectorCallback(hector_mapping::ResetMapping::Request  &req,
+                                             hector_mapping::ResetMapping::Response &res)
+{
+  // Reset map
+  ROS_INFO("HectorSM Reset map");
+  slamProcessor->reset();
+
+  // Reset pose
+  this->resetPose(req.initial_pose);
+
+  // Unpause node (in case it is paused)
+  this->toggleMappingPause(false);
+
+  // Return success
+  return true;
+}
+
+bool HectorMappingRos::pauseMapCallback(std_srvs::SetBool::Request  &req,
+                                        std_srvs::SetBool::Response &res)
+{
+  this->toggleMappingPause(req.data);
+  res.success = true;
+  return true;
+}
+
+void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex)
+{
+  nav_msgs::GetMap::Response& map_ (mapPublisher.map_);
+
+  //only update map if it changed
+  if (lastGetMapUpdateIndex != gridMap.getUpdateIndex())
+  {
+
+    int sizeX = gridMap.getSizeX();
+    int sizeY = gridMap.getSizeY();
+
+    int size = sizeX * sizeY;
+
+    std::vector<int8_t>& data = map_.map.data;
+
+    //std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop
+    memset(&data[0], -1, sizeof(int8_t) * size);
+
+    if (mapMutex)
+    {
+      mapMutex->lockMap();
+    }
+
+    for(int i=0; i < size; ++i)
+    {
+      if(gridMap.isFree(i))
+      {
+        data[i] = 0;
+      }
+      else if (gridMap.isOccupied(i))
+      {
+        data[i] = 100;
+      }
+    }
+
+    lastGetMapUpdateIndex = gridMap.getUpdateIndex();
+
+    if (mapMutex)
+    {
+      mapMutex->unlockMap();
+    }
+  }
+
+  map_.map.header.stamp = timestamp;
+
+  mapPublisher.mapPublisher_.publish(map_.map);
+}
+
+void HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap)
+{
+  size_t size = scan.ranges.size();
+
+  float angle = scan.angle_min;
+
+  dataContainer.clear();
+
+  dataContainer.setOrigo(Eigen::Vector2f::Zero());
+
+  float maxRangeForContainer = scan.range_max - 0.1f;
+
+  for (size_t i = 0; i < size; ++i)
+  {
+    float dist = scan.ranges[i];
+
+    if ( (dist > scan.range_min) && (dist < maxRangeForContainer))
+    {
+      dist *= scaleToMap;
+      dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist));
+    }
+
+    angle += scan.angle_increment;
+  }
+}
+
+void HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
+{
+  size_t size = pointCloud.points.size();
+  //ROS_INFO("size: %d", size);
+
+  dataContainer.clear();
+
+  tf::Vector3 laserPos (laserTransform.getOrigin());
+  dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
+
+  for (size_t i = 0; i < size; ++i)
+  {
+
+    const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
+
+    float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
+
+    if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
+
+      if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
+        continue;
+      }
+
+      tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
+
+      float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
+
+      if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
+      {
+        dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
+      }
+    }
+  }
+}
+
+void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap)
+{
+  Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero()));
+  mapOrigin.array() -= gridMap.getCellLength()/1000*0.5f;
+
+  map_.map.info.origin.position.x = mapOrigin.x();
+  map_.map.info.origin.position.y = mapOrigin.y();
+  map_.map.info.origin.orientation.w = 1.0;
+
+  map_.map.info.resolution = gridMap.getCellLength();
+
+  map_.map.info.width = gridMap.getSizeX();
+  map_.map.info.height = gridMap.getSizeY();
+
+  map_.map.header.frame_id = p_map_frame_;
+  map_.map.data.resize(map_.map.info.width * map_.map.info.height);
+}
+
+/*
+void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map)
+{
+  float cell_length = map.info.resolution;
+  Eigen::Vector2f mapOrigin (map.info.origin.position.x + cell_length*0.5f,
+                             map.info.origin.position.y + cell_length*0.5f);
+
+  int map_size_x = map.info.width;
+  int map_size_y = map.info.height;
+
+  slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, Eigen::Vector2f(0.0f, 0.0f), 1, hectorDrawings, debugInfoProvider);
+}
+*/
+
+
+void HectorMappingRos::publishMapLoop(double map_pub_period)
+{
+  ros::Rate r(1.0 / map_pub_period);
+  while(ros::ok())
+  {
+    //ros::WallTime t1 = ros::WallTime::now();
+    ros::Time mapTime (ros::Time::now());
+    //publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime);
+    //publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime);
+    publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0));
+
+    //ros::WallDuration t2 = ros::WallTime::now() - t1;
+
+    //std::cout << "time s: " << t2.toSec();
+    //ROS_INFO("HectorSM ms: %4.2f", t2.toSec()*1000.0f);
+
+    r.sleep();
+  }
+}
+
+void HectorMappingRos::staticMapCallback(const nav_msgs::OccupancyGrid& map)
+{
+
+}
+
+void HectorMappingRos::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
+{
+  this->resetPose(msg->pose.pose);
+}
+
+void HectorMappingRos::toggleMappingPause(bool pause)
+{
+  // Pause/unpause
+  if (pause && !pause_scan_processing_)
+  {
+    ROS_INFO("[HectorSM]: Mapping paused");
+  }
+  else if (!pause && pause_scan_processing_)
+  {
+    ROS_INFO("[HectorSM]: Mapping no longer paused");
+  }
+  pause_scan_processing_ = pause;
+}
+
+void HectorMappingRos::resetPose(const geometry_msgs::Pose &pose)
+{
+  initial_pose_set_ = true;
+  initial_pose_ = Eigen::Vector3f(pose.position.x, pose.position.y, util::getYawFromQuat(pose.orientation));
+  ROS_INFO("[HectorSM]: Setting initial pose with world coords x: %f y: %f yaw: %f",
+           initial_pose_[0], initial_pose_[1], initial_pose_[2]);
+}

Localizations/Libraries/Ros/hector_slam/hector_mapping/src/PoseInfoContainer.cpp

@@ -0,0 +1,70 @@
+//=================================================================================================
+// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_mapping/PoseInfoContainer.h"
+
+void PoseInfoContainer::update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id)
+{
+  //Fill stampedPose
+  std_msgs::Header& header = stampedPose_.header;
+  header.stamp = stamp;
+  header.frame_id = frame_id;
+
+  geometry_msgs::Pose& pose = stampedPose_.pose;
+  pose.position.x = slamPose.x();
+  pose.position.y = slamPose.y();
+
+  pose.orientation.w = cos(slamPose.z()*0.5f);
+  pose.orientation.z = sin(slamPose.z()*0.5f);
+
+  //Fill covPose
+  //geometry_msgs::PoseWithCovarianceStamped covPose;
+  covPose_.header = header;
+  covPose_.pose.pose = pose;
+
+  boost::array<double, 36>& cov(covPose_.pose.covariance);
+
+  cov[0] = static_cast<double>(slamCov(0,0));
+  cov[7] = static_cast<double>(slamCov(1,1));
+  cov[35] = static_cast<double>(slamCov(2,2));
+
+  double xyC = static_cast<double>(slamCov(0,1));
+  cov[1] = xyC;
+  cov[6] = xyC;
+
+  double xaC = static_cast<double>(slamCov(0,2));
+  cov[5] = xaC;
+  cov[30] = xaC;
+
+  double yaC = static_cast<double>(slamCov(1,2));
+  cov[11] = yaC;
+  cov[31] = yaC;
+
+  //Fill tf tansform
+  tf::poseMsgToTF(pose, poseTransform_);
+}

Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main.cpp

@@ -0,0 +1,44 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "hector_mapping/HectorMappingRos.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "hector_slam");
+
+  HectorMappingRos sm;
+
+  ros::spin();
+
+  return(0);
+}
+

Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main_mapper.cpp

@@ -0,0 +1,44 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "HectorMapperRos.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "hector_slam");
+
+  HectorMapperRos sm;
+
+  ros::spin();
+
+  return(0);
+}
+

Localizations/Libraries/Ros/hector_slam/hector_mapping/srv/ResetMapping.srv

@@ -0,0 +1,2 @@
+geometry_msgs/Pose initial_pose
+---

Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CHANGELOG.rst

@@ -0,0 +1,46 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_marker_drawing
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+* Use the FindEigen3.cmake module provided by Eigen
+* Contributors: Johannes Meyer
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+* fixed cmake find for eigen in indigo
+* Contributors: Alexander Stumpf
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_marker_drawing)
+
+find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs)
+
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS roscpp visualization_msgs
+  DEPENDS EIGEN3
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/DrawInterface.h

@@ -0,0 +1,50 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 drawinterface_h__
+#define drawinterface_h__
+
+#include <Eigen/Core>
+#include <string>
+
+class DrawInterface{
+public:
+  virtual void setNamespace(const std::string& ns) = 0;
+
+  virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0;
+  virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0;
+  virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0;
+  virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix) = 0;
+
+  virtual void setScale(double scale) = 0;
+  virtual void setColor(double r, double g, double b, double a = 1.0) = 0;
+
+  virtual void sendAndResetData() = 0;
+};
+
+#endif

Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/HectorDrawings.h

@@ -0,0 +1,242 @@
+//=================================================================================================
+// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
+// 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 Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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 "DrawInterface.h"
+//#include "util/UtilFunctions.h"
+
+#include "ros/ros.h"
+
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+
+
+#include <visualization_msgs/MarkerArray.h>
+
+class HectorDrawings : public DrawInterface
+{
+public:
+
+  HectorDrawings()
+  {
+    idCounter = 0;
+    maxId = 0;
+
+    ros::NodeHandle nh_;
+
+    markerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1, true);
+    markerArrayPublisher_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1, true);
+
+    tempMarker.header.frame_id = "map";
+    tempMarker.ns = "marker";
+
+    this->setScale(1.0);
+    this->setColor(1.0, 1.0, 1.0);
+
+    tempMarker.action = visualization_msgs::Marker::ADD;
+  };
+
+  virtual void setNamespace(const std::string& ns)
+  {
+    tempMarker.ns = ns;
+  }
+
+  virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame)
+  {
+    tempMarker.id = idCounter++;
+
+    tempMarker.pose.position.x = pointWorldFrame.x();
+    tempMarker.pose.position.y = pointWorldFrame.y();
+
+    tempMarker.pose.orientation.w = 0.0;
+    tempMarker.pose.orientation.z = 0.0;
+    tempMarker.type = visualization_msgs::Marker::CUBE;
+
+    //markerPublisher_.publish(tempMarker);
+
+    markerArray.markers.push_back(tempMarker);
+  }
+
+  virtual void drawArrow(const Eigen::Vector3f& poseWorld)
+  {
+    tempMarker.id = idCounter++;
+
+    tempMarker.pose.position.x = poseWorld.x();
+    tempMarker.pose.position.y = poseWorld.y();
+
+    tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f);
+    tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f);
+
+    tempMarker.type = visualization_msgs::Marker::ARROW;
+
+    //markerPublisher_.publish(tempMarker);
+
+    markerArray.markers.push_back(tempMarker);
+
+  }
+
+  virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix)
+  {
+
+    tempMarker.pose.position.x = mean[0];
+    tempMarker.pose.position.y = mean[1];
+
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covMatrix);
+
+    const Eigen::Vector2f& eigValues (eig.eigenvalues());
+    const Eigen::Matrix2f& eigVectors (eig.eigenvectors());
+
+    float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));
+
+    tempMarker.type = visualization_msgs::Marker::CYLINDER;
+
+    double lengthMajor = sqrt(eigValues[0]);
+    double lengthMinor = sqrt(eigValues[1]);
+
+    tempMarker.scale.x = lengthMajor;
+    tempMarker.scale.y = lengthMinor;
+    tempMarker.scale.z = 0.001;
+
+
+    tempMarker.pose.orientation.w = cos(angle*0.5);
+    tempMarker.pose.orientation.z = sin(angle*0.5);
+
+    tempMarker.id = idCounter++;
+    markerArray.markers.push_back(tempMarker);
+  }
+
+  virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix)
+  {
+    tempMarker.type = visualization_msgs::Marker::SPHERE;
+
+    tempMarker.color.r = 0.0;
+    tempMarker.color.a = 0.5;
+
+    tempMarker.pose.position.x = mean[0];
+    tempMarker.pose.position.y = mean[1];
+    tempMarker.pose.position.z = mean[2];
+
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(covMatrix);
+
+    const Eigen::Vector3f& eigValues (eig.eigenvalues());
+    const Eigen::Matrix3f& eigVectors (eig.eigenvectors());
+
+
+    Eigen::Matrix3f eigVectorsFlipped;
+    eigVectorsFlipped.col(0) = eigVectors.col(2);
+    eigVectorsFlipped.col(1) = eigVectors.col(1);
+    eigVectorsFlipped.col(2) = eigVectors.col(0);
+
+    if (eigVectorsFlipped.determinant() < 0){
+      eigVectorsFlipped.col(2) = -eigVectorsFlipped.col(2);
+    }
+
+    Eigen::Quaternionf quaternion (eigVectorsFlipped);
+
+    //std::cout << "\neigVec:\n" << eigVectors << "\n";
+    //std::cout << "\neigVecFlipped:\n" << eigVectorsFlipped << "\n";
+
+    //std::cout << "\now:" << quaternion.w() << " x:" << quaternion.x() << " y:" << quaternion.y() << " z:" << quaternion.z() << "\n";
+
+
+    tempMarker.pose.orientation.w = quaternion.w();
+    tempMarker.pose.orientation.x = quaternion.x();
+    tempMarker.pose.orientation.y = quaternion.y();
+    tempMarker.pose.orientation.z = quaternion.z();
+
+    tempMarker.scale.x = sqrt(eigValues[2]);
+    tempMarker.scale.y = sqrt(eigValues[1]);
+    tempMarker.scale.z = sqrt(eigValues[0]);
+
+    tempMarker.id = idCounter++;
+    markerArray.markers.push_back(tempMarker);
+
+
+  }
+
+  virtual void setScale(double scale)
+  {
+    tempMarker.scale.x = scale;
+    tempMarker.scale.y = scale;
+    tempMarker.scale.z = scale;
+  }
+
+  virtual void setColor(double r, double g, double b, double a = 1.0)
+  {
+    tempMarker.color.r = r;
+    tempMarker.color.g = g;
+    tempMarker.color.b = b;
+    tempMarker.color.a = a;
+  }
+
+  virtual void addMarker(visualization_msgs::Marker marker) {
+    if (marker.id == 0) marker.id = idCounter++;
+    if (marker.ns.empty()) marker.ns = tempMarker.ns;
+    markerArray.markers.push_back(marker);
+  }
+
+  virtual void addMarkers(visualization_msgs::MarkerArray markers) {
+    for(visualization_msgs::MarkerArray::_markers_type::iterator it = markers.markers.begin(); it != markers.markers.end(); ++it) {
+      visualization_msgs::Marker &marker = *it;
+      addMarker(marker);
+    }
+  }
+
+  virtual void sendAndResetData()
+  {
+    allMarkers.markers.insert(allMarkers.markers.end(), markerArray.markers.begin(), markerArray.markers.end());
+    markerArrayPublisher_.publish(markerArray);
+    markerArray.markers.clear();
+    if (idCounter > maxId) maxId = idCounter;
+    idCounter = 0;
+  }
+
+  void setTime(const ros::Time& time)
+  {
+    tempMarker.header.stamp = time;
+  }
+
+  void reset()
+  {
+    for(visualization_msgs::MarkerArray::_markers_type::iterator it = allMarkers.markers.begin(); it != allMarkers.markers.end(); ++it)
+    {
+      it->action = visualization_msgs::Marker::DELETE;
+    }
+    markerArrayPublisher_.publish(allMarkers);
+    allMarkers.markers.clear();
+  }
+
+  ros::Publisher markerPublisher_;
+  ros::Publisher markerArrayPublisher_;
+
+  visualization_msgs::Marker tempMarker;
+  visualization_msgs::MarkerArray markerArray;
+  visualization_msgs::MarkerArray allMarkers;
+
+  int idCounter;
+  int maxId;
+};

Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_marker_drawing</name>
+  <version>0.5.2</version>
+  <description>
+    hector_marker_drawing provides convenience functions for easier publishing of visualization markers.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_marker_drawing</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>eigen</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>eigen</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CHANGELOG.rst

@@ -0,0 +1,47 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_nav_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+* hector_nav_msgs: removed yaw member from GetNormal response
+  yaw is implicitly given by the normal vector
+* Contributors: Dorothea Koert
+
+0.3.3 (2014-06-15)
+------------------
+* added GetNormal service, that returns normal and orientation of an occupied cell
+* Contributors: Dorothea Koert
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_nav_msgs)
+
+find_package(catkin REQUIRED COMPONENTS nav_msgs geometry_msgs message_generation std_msgs)
+
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetDistanceToObstacle.srv
+  GetRecoveryInfo.srv
+  GetRobotTrajectory.srv
+  GetSearchPosition.srv
+  GetNormal.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs nav_msgs std_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS nav_msgs geometry_msgs message_runtime std_msgs
+)

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/package.xml

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>hector_nav_msgs</name>
+  <version>0.5.2</version>
+  <description>
+    hector_nav_msgs contains messages and services used in the hector_slam stack.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_nav_msgs</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetDistanceToObstacle.srv

@@ -0,0 +1,11 @@
+# Returns the distance to the next obstacle from the origin of frame point.header.frame_id
+# in the direction of the point
+#
+# All units are meters.
+
+geometry_msgs/PointStamped point
+---
+float32 distance
+geometry_msgs/PointStamped end_point
+
+

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetNormal.srv

@@ -0,0 +1,3 @@
+geometry_msgs/PointStamped point
+---
+geometry_msgs/Vector3 normal

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRecoveryInfo.srv

@@ -0,0 +1,11 @@
+# Returns the path travelled to get to req_pose (pose determined by request_time) 
+# up to request_radius away from req_pose.
+#
+
+time request_time
+float64 request_radius 
+---
+nav_msgs/Path trajectory_radius_entry_pose_to_req_pose
+geometry_msgs/PoseStamped radius_entry_pose
+geometry_msgs/PoseStamped req_pose
+

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRobotTrajectory.srv

@@ -0,0 +1,8 @@
+# Returns the distance to the next obstacle from the origin of frame point.header.frame_id
+# in the direction of the point
+#
+# All units are meters.
+
+---
+nav_msgs/Path trajectory
+

Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetSearchPosition.srv

@@ -0,0 +1,7 @@
+#Returns a suggested search/observation position for an object of interest located at ooi_pose
+
+geometry_msgs/PoseStamped ooi_pose
+float32 distance
+---
+geometry_msgs/PoseStamped search_pose
+

Localizations/Libraries/Ros/hector_slam/hector_slam/CHANGELOG.rst

@@ -0,0 +1,42 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_slam
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+* Added hector_geotiff_launch to hector_slam metapackge.
+* Contributors: Stefan Fabian
+
+0.5.0 (2020-12-17)
+------------------
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam

Localizations/Libraries/Ros/hector_slam/hector_slam/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(hector_slam)
+find_package(catkin REQUIRED)
+catkin_metapackage()

Localizations/Libraries/Ros/hector_slam/hector_slam/package.xml

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_slam</name>
+  <version>0.5.2</version>
+  <description>
+    The hector_slam metapackage that installs hector_mapping and related packages.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <url type="website">http://ros.org/wiki/hector_slam</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <run_depend>hector_compressed_map_transport</run_depend>
+  <run_depend>hector_geotiff</run_depend>
+  <run_depend>hector_geotiff_launch</run_depend>
+  <run_depend>hector_geotiff_plugins</run_depend>
+  <run_depend>hector_imu_attitude_to_tf</run_depend>
+  <run_depend>hector_map_server</run_depend>
+  <run_depend>hector_map_tools</run_depend>
+  <run_depend>hector_mapping</run_depend>
+  <run_depend>hector_marker_drawing</run_depend>
+  <run_depend>hector_nav_msgs</run_depend>
+  <run_depend>hector_slam_launch</run_depend>
+  <run_depend>hector_trajectory_server</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <metapackage/>
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CHANGELOG.rst

@@ -0,0 +1,52 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_slam_launch
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.2 (2021-04-08)
+------------------
+
+0.5.1 (2021-01-15)
+------------------
+* Updated paths to launch files.
+* Contributors: Stefan Fabian
+
+0.5.0 (2020-12-17)
+------------------
+* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
+  Clean up for noetic release.
+* Bump CMake version to avoid CMP0048 warning
+* Contributors: Marius Schnaubelt, Stefan Fabian
+
+0.4.1 (2020-05-15)
+------------------
+
+0.3.6 (2019-10-31)
+------------------
+
+0.3.5 (2016-06-24)
+------------------
+
+0.3.4 (2015-11-07)
+------------------
+* Removes trailing spaces and fixes indents
+* "rviz_cfg" directory in the repository version.
+* Contributors: YuK_Ota, dani-carbonell
+
+0.3.3 (2014-06-15)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+* Add arguments to launch files for specifying geotiff file path
+* Update rviz config to have proper default displays
+* deleted quadrotor_uav.launch (moved to hector_quadrotor)
+* Contributors: Johannes Meyer, Stefan Kohlbrecher
+
+0.3.1 (2013-10-09)
+------------------
+* updated rviz config to the new .rviz format
+* added changelogs
+
+0.3.0 (2013-08-08)
+------------------
+* catkinized hector_slam