Add Dockerfile

.dockerignore

@@ -0,0 +1,8 @@
+################################################################################
+# Repo
+
+.git/*
+.dockerignore
+.gitignore
+**Dockerfile
+**.Dockerfile

.gitignore

@@ -0,0 +1,52 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+

Dockerfile

@@ -0,0 +1,162 @@
+ARG FROM_IMAGE=ros:eloquent
+
+# multi-stage for caching
+FROM $FROM_IMAGE AS cache
+
+# clone underlay source
+ENV UNDERLAY_WS /opt/underlay_ws
+RUN mkdir -p $UNDERLAY_WS/src
+WORKDIR $UNDERLAY_WS
+COPY ./install/underlay.repos ./
+RUN vcs import src < underlay.repos
+
+# copy overlay source
+ENV OVERLAY_WS /opt/overlay_ws
+RUN mkdir -p $OVERLAY_WS/src
+WORKDIR $OVERLAY_WS
+COPY ./install/overlay.repos ./
+RUN vcs import src < overlay.repos
+
+# copy wrapper source
+ENV WRAPPER_WS /opt/wrapper_ws
+RUN mkdir -p $WRAPPER_WS/src
+WORKDIR $WRAPPER_WS
+# COPY ./install/wrapper.repos ./
+# RUN vcs import src < wrapper.repos
+COPY ./ src/MIT-SPARK/Kimera-VIO-ROS2
+
+# copy manifests for caching
+WORKDIR /opt
+RUN find ./ -name "package.xml" | \
+      xargs cp --parents -t /tmp
+    # && find ./ -name "COLCON_IGNORE" | \
+    #   xargs cp --parents -t /tmp
+
+# multi-stage for building
+FROM $FROM_IMAGE AS build
+
+# install CI dependencies
+RUN apt-get update && apt-get install -q -y \
+      ccache \
+      lcov \
+    && rm -rf /var/lib/apt/lists/*
+
+# install opencv dependencies
+RUN apt-get update && apt-get install -y \
+      gfortran \
+      libatlas-base-dev \
+      libgtk-3-dev \
+      libjpeg-dev \
+      libpng-dev \
+      libtiff-dev \
+      libvtk6-dev \
+      unzip \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy underlay manifests
+ENV UNDERLAY_WS /opt/underlay_ws
+COPY --from=cache /tmp/underlay_ws $UNDERLAY_WS
+WORKDIR $UNDERLAY_WS
+
+# install underlay dependencies
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy underlay source
+COPY --from=cache $UNDERLAY_WS ./
+
+# build underlay source
+ARG UNDERLAY_MIXINS="release"
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $UNDERLAY_MIXINS \
+      --cmake-args \
+        --no-warn-unused-cli \
+        -DGTSAM_BUILD_TESTS=OFF \
+        -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+        -DGTSAM_BUILD_UNSTABLE=ON \
+        -DGTSAM_POSE3_EXPMAP=ON \
+        -DGTSAM_ROT3_EXPMAP=ON \
+        -DOPENCV_EXTRA_MODULES_PATH=$UNDERLAY_WS/src/opencv/opencv/contrib/modules
+      # --event-handlers console_direct+
+
+# copy overlay manifests
+ENV OVERLAY_WS /opt/overlay_ws
+COPY --from=cache /tmp/overlay_ws $OVERLAY_WS
+WORKDIR $OVERLAY_WS
+
+# install overlay dependencies
+RUN . $UNDERLAY_WS/install/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths \
+        src \
+        $UNDERLAY_WS/src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy overlay source
+COPY --from=cache $OVERLAY_WS ./
+
+# build overlay source
+ARG OVERLAY_MIXINS="release ccache"
+RUN . $UNDERLAY_WS/install/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $OVERLAY_MIXINS \
+      --cmake-args \
+        --no-warn-unused-cli \
+        -DCMAKE_CXX_FLAGS="\
+          -Wno-parentheses \
+          -Wno-reorder \
+          -Wno-sign-compare \
+          -Wno-unused-but-set-variable \
+          -Wno-unused-function \
+          -Wno-unused-parameter \
+          -Wno-unused-value \
+          -Wno-unused-variable"
+      # --event-handlers console_direct+
+
+# copy wrapper manifests
+ENV WRAPPER_WS /opt/wrapper_ws
+COPY --from=cache /tmp/wrapper_ws $WRAPPER_WS
+WORKDIR $WRAPPER_WS
+
+# install wrapper dependencies
+RUN . $OVERLAY_WS/install/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths \
+        src \
+        $UNDERLAY_WS/src \
+        $OVERLAY_WS/src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy wrapper source
+COPY --from=cache $WRAPPER_WS ./
+
+# build wrapper source
+ARG WRAPPER_MIXINS="release ccache"
+RUN . $OVERLAY_WS/install/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $WRAPPER_MIXINS \
+      --cmake-args \
+        --no-warn-unused-cli \
+        -DCMAKE_CXX_FLAGS="\
+          -Wno-sign-compare \
+          -Wno-unused-value \
+          -Wno-unused-variable \
+          -Wno-unused-but-set-variable \
+          -Wno-reorder \
+          -Wno-parentheses \
+          -Wno-unused-parameter"
+      # --event-handlers console_direct+
+
+# source wrapper from entrypoint
+RUN sed --in-place \
+      's|^source .*|source "$WRAPPER_WS/install/setup.bash"|' \
+      /ros_entrypoint.sh

install/overlay.repos

@@ -0,0 +1,13 @@
+repositories:
+  MIT-SPARK/Kimera-RPGO:
+    type: git
+    # url: https://github.com/MIT-SPARK/Kimera-RPGO.git
+    # version: master
+    url: https://github.com/ruffsl/Kimera-RPGO.git
+    version: patch-1
+  MIT-SPARK/Kimera-VIO:
+    type: git
+    # url: https://github.com/MIT-SPARK/Kimera-VIO.git
+    # version: master
+    url: https://github.com/ruffsl/Kimera-VIO.git
+    version: patch-3

install/underlay.repos

@@ -0,0 +1,27 @@
+repositories:
+  borglab/gtsam:
+    type: git
+    url: https://github.com/borglab/gtsam.git
+    version: develop
+  laurentkneip/opengv:
+    type: git
+    # url: https://github.com/laurentkneip/opengv.git
+    # version: master
+    url: https://github.com/ruffsl/opengv.git
+    version: patch-1
+  dorian3d/DBoW2:
+    type: git
+    # url: https://github.com/dorian3d/DBoW2.git
+    # version: master
+    url: https://github.com/ruffsl/DBoW2.git
+    version: patch-1
+  opencv/opencv:
+    type: git
+    # url: https://github.com/opencv/opencv.git
+    # version: master
+    url: https://github.com/ruffsl/opencv.git
+    version: patch-1
+  opencv/opencv/contrib:
+    type: git
+    url: https://github.com/opencv/opencv_contrib.git
+    version: 3.3.1

install/wrapper.repos

@@ -0,0 +1,15 @@
+repositories:
+  MIT-SPARK/Kimera-VIO-ROS2:
+    type: git
+    url: https://github.com/MIT-SPARK/Kimera-VIO-ROS2.git
+    version: master
+  # ToniRV/mesh_rviz_plugins:
+  #   type: git
+  #   url: https://github.com/ToniRV/mesh_rviz_plugins.git
+  #   version: master
+  #   # url: https://github.com/ruffsl/mesh_rviz_plugins.git
+  #   # version: patch-1
+  # ToniRV/kimera_rviz_markers:
+  #   type: git
+  #   url: https://github.com/ToniRV/kimera_rviz_markers.git
+  #   version: master

kimera_common/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.5)
+project(kimera_common NONE)
+
+find_package(ament_cmake_core REQUIRED)
+
+ament_package(
+    CONFIG_EXTRAS "kimera_common-extras.cmake"
+)
+
+install(
+    DIRECTORY cmake
+    DESTINATION share/${PROJECT_NAME}
+)

kimera_common/cmake/kimera_package.cmake

@@ -0,0 +1,22 @@
+#
+# Standard Kimera project setup
+#
+# @public
+#
+macro(kimera_package)
+
+  # Default to C++14
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+  endif()
+
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(
+      -Wall
+      -Wextra
+      -Wpedantic
+      -Werror
+      -Wdeprecated
+    )
+  endif()
+endmacro()

kimera_common/kimera_common-extras.cmake

@@ -0,0 +1,3 @@
+set(AMENT_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":")
+
+include("${kimera_common_DIR}/kimera_package.cmake")

kimera_common/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>kimera_common</name>
+  <version>0.0.1</version>
+  <description>Common support functionality used throughout the Kimera stack</description>
+  <maintainer email="roxfoxpox@gmail.com">Ruffin White</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake_core</buildtool_depend>
+
+  <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

kimera_graph_msgs/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 3.5)
+project(kimera_graph_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(kimera_common REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+kimera_package()
+
+set(msg_files
+  "msg/PoseGraph.msg"
+  "msg/PoseGraphEdge.msg"
+  "msg/PoseGraphNode.msg"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
+  DEPENDENCIES
+    builtin_interfaces
+    geometry_msgs
+    std_msgs
+)
+
+ament_package()

kimera_graph_msgs/msg/PoseGraph.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+# Nodes and edges
+PoseGraphNode[] nodes
+PoseGraphEdge[] edges

kimera_graph_msgs/msg/PoseGraphEdge.msg

@@ -0,0 +1,16 @@
+std_msgs/Header header
+
+uint64 key_from
+uint64 key_to
+
+int32 type
+
+# Type enums
+int32 ODOM      = 0
+int32 LOOPCLOSE = 1
+int32 LANDMARK  = 2
+int32 REJECTED_LOOPCLOSE = 3
+
+# Transforms in ede
+geometry_msgs/Pose pose
+float64[36] covariance

kimera_graph_msgs/msg/PoseGraphNode.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+uint64 key
+
+geometry_msgs/Pose pose