[M3RSM] Implementation of basic classes

.gitignore

@@ -1 +1,5 @@
 snap_ws/*
+
+build/
+log/*
+install/*

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.10.2)
 project(slam_toolbox)
 
 set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
@@ -9,6 +9,7 @@ cmake_policy(SET CMP0077 NEW)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/CMake/")
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/karto_sdk/cmake)
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/m3rsm/cmake)
 
 find_package(ament_cmake REQUIRED)
 find_package(message_filters REQUIRED)

docker/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:foxy-ros-base-focal
+
+# USE BASH
+SHELL ["/bin/bash", "-c"]
+
+# RUN LINE BELOW TO REMOVE debconf ERRORS (MUST RUN BEFORE ANY apt-get CALLS)
+RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+
+RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends apt-utils
+
+# slam_toolbox
+RUN mkdir -p colcon_ws/src
+RUN cd colcon_ws/src 
+# RUN git clone -b foxy-devel https://github.com/SteveMacenski/slam_toolbox.git
+
+#RUN source /opt/ros/foxy/setup.bash \
+#    && cd colcon_ws \
+#    && rosdep update \
+#    && rosdep install -y -r --from-paths src --ignore-src --rosdistro=foxy -y
+
+#RUN source /opt/ros/foxy/setup.bash \
+#    && cd colcon_ws/ \
+#    && colcon build  --cmake-args=-DCMAKE_BUILD_TYPE=Release \
+    # && colcon test
+
+# RUN apt-get update && apt-get install -y tmux
+
+CMD /bin/bash

docker/build.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+docker build -t m3rsm-image .

docker/inside_docker_build.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+source /opt/ros/foxy/setup.bash
+cd /colcon_ws/
+colcon build  --cmake-args=-DCMAKE_BUILD_TYPE=Release

docker/inside_docker_init.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+apt-get update
+source /opt/ros/foxy/setup.bash
+cd /colcon_ws
+rosdep update
+rosdep install -y -r --from-paths src --ignore-src --rosdistro=foxy -y

docker/inside_docker_test.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+source /opt/ros/foxy/setup.bash  && cd /colcon_ws/  && colcon build  --cmake-args=-DCMAKE_BUILD_TYPE=Release && colcon test && colcon test-result

docker/run.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+CONTAINER=m3rsm-image
+IMAGE_NAME=m3rsm-image
+
+SCRIPTS_DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+REPO_DIR=`readlink -f ${SCRIPTS_DIR}/../`
+
+DOCKER_MOUNT_ARGS="-v ${REPO_DIR}/:/colcon_ws/src"
+
+xhost +
+docker run --name ${IMAGE_NAME} --privileged --rm \
+    ${DOCKER_MOUNT_ARGS} \
+    -e USER=$USER -e USERID=$UID \
+    --net=host \
+    -it ${CONTAINER}

lib/karto_sdk/CMakeLists.txt

@@ -28,7 +28,7 @@ include_directories(include ${catkin_INCLUDE_DIRS}
 add_definitions(${EIGEN3_DEFINITIONS})
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
-add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp)
+add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp src/M3RScanMatcher.cpp)
 ament_target_dependencies(kartoSlamToolbox ${dependencies})
 target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})
 
@@ -46,3 +46,6 @@ ament_export_libraries(kartoSlamToolbox)
 ament_export_include_directories(include)
 ament_export_dependencies(${dependencies})
 ament_package()
+
+enable_testing()
+add_subdirectory(m3rsm_test)

lib/karto_sdk/include/karto_sdk/M3RScanMatcher.h

@@ -0,0 +1,122 @@
+#ifndef M3RSM__H
+#define M3RSM__H
+
+#include <stdint.h>
+#include <map>
+#include <vector>
+#include <unordered_map>
+#include <queue>
+#include <algorithm>
+#include <chrono>
+#include <utility>
+#include <string>
+
+#include "Eigen/Core"
+#include "rclcpp/rclcpp.hpp"
+#include "karto_sdk/Karto.h"
+#include "karto_sdk/Mapper.h"
+
+namespace {
+  typedef std::vector<std::vector<uint8_t>> DataMatrix;
+}
+
+namespace karto
+{
+
+class M3RSM_scan;
+
+// TODO: Implement this class.
+class LookupTable {
+
+  public:
+    // Builds highest resolution lookup table from base scans
+    LookupTable(const std::vector<LocalizedRangeScan>& base_scans, int decimation_factor, int kernel_width);
+    // Copy constructor
+    LookupTable(const LookupTable& table);
+    // Builds lower resolution lookup table from higher resolution lookup table
+    static LookupTable GetLowerResTable(const LookupTable& higher_res_table);
+    double computeCost(const LocalizedRangeScan& scan);
+    bool isMaxResolution();
+    int getTableSize();
+
+  private:
+    DataMatrix data_;
+    bool is_max_res_;
+    int decimation_factor_;
+    int kernel_width_;
+
+};  // class LookupTable
+
+class MockLookupTable {
+
+  public:
+    MockLookupTable(const DataMatrix& data, int decimation_factor, int kernel_width);
+    // Builds lower resolution lookup table from higher resolution lookup table
+    MockLookupTable(const MockLookupTable& table);
+    static MockLookupTable GetLowerResTable(const MockLookupTable& higher_res_table);
+    double computeCost(const std::vector<std::pair<int, int>>& occupied_cells);
+    bool isMaxResolution();
+    int getTableSize();
+    DataMatrix data_;
+    bool is_max_res_;
+    int decimation_factor_;
+    int kernel_width_;
+};  // class MockLookupTable
+
+class LookupTableManager {
+
+  public:
+    // TODO: Implement this method when integrating with slam_toolbox codebase.
+    // LookupTableManager(const std::vector<LocalizedRangeScan>& base_scans);
+    LookupTableManager(const DataMatrix& full_res_matrix);
+    MockLookupTable getTable(int resolution);
+    int getAmountOfResolutions();
+
+  private:
+    // Lookup tables ordered by resolution.
+    // Each vector contains a vector of same-resolution lookup tables.
+    // TODO: Use this attribute when integrating with slam_toolbox.
+    //std::vector<std::vector<LookupTable>> tables_;
+    std::vector<MockLookupTable> tables_;
+    int n_resolutions_;
+
+};  // class LookupTableManager
+
+class EvaluationTask {
+  public:
+    EvaluationTask(const LookupTable& lookup_table);
+    double computeCost();
+    std::vector<EvaluationTask> generateChildrenTasks();
+    bool isMaxResolution();
+    Pose2 getPose();
+
+  private:
+    std::shared_ptr<LookupTable> lookup_table_;
+};  // class EvaluationTask
+
+class SearchHeap {
+
+  public:
+    SearchHeap(const LocalizedRangeScan& received_scan, const LookupTableManager& lookup_table_manager);
+    Pose2 getEstimatedPose(); 
+
+  private:
+    std::shared_ptr<LocalizedRangeScan> received_scan_;
+    std::shared_ptr<LookupTableManager> lookup_table_manager_;
+    std::vector<EvaluationTask> tasks;
+
+};  // class SearchHeap
+
+
+class M3RScanMatcher {
+
+public:
+  M3RScanMatcher();
+  Pose2 MatchScan(LocalizedRangeScan received_scan, std::vector<LocalizedRangeScan> potential_scans);
+
+  private:
+};  // M3RScanMatcher
+
+} // namespace karto 
+
+#endif  // M3RSM__H

lib/karto_sdk/m3rsm_test/CMakeLists.txt

@@ -0,0 +1,66 @@
+include_directories (
+  ${PROJECT_SOURCE_DIR}/m3rsm_test/gtest/include
+  ${PROJECT_SOURCE_DIR}/m3rsm_test/gtest
+  ${PROJECT_SOURCE_DIR}/m3rsm_test
+  ${PROJECT_BINARY_DIR}
+)
+
+# configure_file (test_config.h.in ${PROJECT_BINARY_DIR}/include/rndf_gazebo_plugin/test_config.h)
+
+# Build gtest
+add_library(gtest STATIC gtest/src/gtest-all.cc)
+add_library(gtest_main STATIC gtest/src/gtest_main.cc)
+target_link_libraries(gtest_main gtest)
+
+set_target_properties(gtest PROPERTIES CXX_CPPLINT "")
+set_target_properties(gtest_main PROPERTIES CXX_CPPLINT "")
+
+set(GTEST_LIBRARY "${PROJECT_BINARY_DIR}/test/libgtest.a")
+set(GTEST_MAIN_LIBRARY "${PROJECT_BINARY_DIR}/test/libgtest_main.a")
+
+execute_process(COMMAND cmake -E remove_directory ${CMAKE_BINARY_DIR}/test_results)
+execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results)
+include_directories(${GTEST_INCLUDE_DIRS})
+
+# This should be migrated to more fine control solution based on set_property APPEND
+# directories. It's present on cmake 2.8.8 while precise version is 2.8.7
+link_directories(${PROJECT_BINARY_DIR}/test)
+include_directories("${PROJECT_SOURCE_DIR}/test/gtest/include")
+
+macro (m3rsm_tests_build)
+  # Build all the tests
+  foreach(GTEST_SOURCE_file ${ARGN})
+    string(REGEX REPLACE ".cc" "" BINARY_NAME ${GTEST_SOURCE_file})
+    message(${BINARY_NAME})
+    set(BINARY_NAME ${TEST_TYPE}_${BINARY_NAME})
+    if(USE_LOW_MEMORY_TESTS)
+      add_definitions(-DUSE_LOW_MEMORY_TESTS=1)
+    endif(USE_LOW_MEMORY_TESTS)
+
+	include_directories(
+		test/gtest/include
+	)
+    add_executable(${BINARY_NAME} ${GTEST_SOURCE_file})
+
+    add_dependencies(${BINARY_NAME}
+      gtest
+    	gtest_main
+    )
+
+    target_link_libraries(${BINARY_NAME}
+    	kartoSlamToolbox
+    )
+
+    target_link_libraries(${BINARY_NAME}
+        libgtest.a
+        libgtest_main.a
+        pthread
+    )
+
+    add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
+        --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
+    set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
+  endforeach()
+endmacro()
+
+add_subdirectory(src)