WIP | ROS2 port

.dockerignore

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

.gitignore

@@ -35,4 +35,8 @@
 *.app
 
 # Vscode files
-.vscode/
+.vscode/
+build/*
+
+# Clion files
+.idea/

CMakeLists.txt

@@ -1,7 +1,44 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(kimera_vio_ros)
 
-find_package(catkin_simple REQUIRED)
+# 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)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pcl_msgs)
+find_package(pose_graph_msgs)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+find_package(cv_bridge REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+# find_package(Gflags REQUIRED)
+# find_package(Glog 0.3.5 REQUIRED)
+find_package(GTSAM REQUIRED)
+# find_package(opengv REQUIRED)
+find_package(OpenCV 3.3.1 REQUIRED)
+# find_package(DBoW2 REQUIRED)
+find_package(kimera_vio REQUIRED)
+find_package(PCL REQUIRED QUIET COMPONENTS common io)
+
+find_package(pose_graph_visualizer)
+
+#find_package(pcl_ros)
+find_package(pcl_conversions)
+
+find_package(message_filters)
+find_package(image_transport)
 
 ### System Dependencies
 find_package(Boost REQUIRED COMPONENTS
@@ -13,15 +50,79 @@ if(NOT TARGET Boost::boost)
   INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}")
 endif()
 
-catkin_simple()
+include_directories(
+  include
+  ${PCL_COMMON_INCLUDE_DIRS}
+)
 
-cs_add_executable(${PROJECT_NAME}
-  src/KimeraVioRos.cpp
-  src/RosDataProvider.cpp
-  src/RosBagDataProvider.cpp
-  src/RosOnlineDataProvider.cpp)
+set(executable_name kimera_vio_node)
+add_executable(${executable_name}
+  src/KimeraVio.cpp
+#  src/stereo.cpp
+  # src/KimeraVioRos.cpp
+  # src/RosDataProviderInterface.cpp
+  # src/RosBagDataProvider.cpp
+  # src/RosOnlineDataProvider.cpp
+)
+
+set(library_name ${PROJECT_NAME})
+add_library(${library_name} SHARED
+  src/KimeraVioNode.cpp
+   src/RosDataProviderInterface.cpp
+  # src/RosBagDataProvider.cpp
+  # src/RosOnlineDataProvider.cpp
+)
+
+set(dependencies
+  geometry_msgs
+  nav_msgs
+  pcl_msgs
+  pose_graph_msgs
+  sensor_msgs
+  std_msgs
+  # gtsam
+  cv_bridge
+  tf2_ros
+  kimera_vio
+  pcl_conversions
+  # pcl_ros
+  pose_graph_visualizer
+  rclcpp
+  message_filters
+  image_transport
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name} ${library_name})
 target_link_libraries(${PROJECT_NAME} kimera_vio)
 
-cs_install()
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+install(TARGETS ${executable_name} ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY param
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
 
-cs_export()
+ament_package()

Dockerfile

@@ -0,0 +1,164 @@
+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 ros source
+ENV ROS_WS /opt/ros_ws
+RUN mkdir -p $ROS_WS/src
+WORKDIR $ROS_WS
+COPY ./install/ros.repos ./
+RUN vcs import src < ros.repos
+COPY ./ src/MIT-SPARK/Kimera-VIO-ROS
+
+# 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 ros manifests
+ENV ROS_WS /opt/ros_ws
+COPY --from=cache /tmp/ros_ws $ROS_WS
+WORKDIR $ROS_WS
+
+# install overlay 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 \
+      --skip-keys "\
+        Boost" \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy overlay source
+COPY --from=cache $ROS_WS ./
+
+# build overlay source
+ARG ROS_MIXINS="release ccache"
+RUN . $OVERLAY_WS/install/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $ROS_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 overlay from entrypoint
+RUN sed --in-place \
+      's|^source .*|source "$ROS_WS/install/setup.bash"|' \
+      /ros_entrypoint.sh

include/kimera_vio_ros/KimeraVioNode.hpp

@@ -0,0 +1,67 @@
+// #include <chrono>
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <thread>
+
+// Still need gflags for parameters in VIO
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "image_transport/subscriber_filter.h"
+
+#include "message_filters/subscriber.h"
+#include "message_filters/synchronizer.h"
+#include "message_filters/sync_policies/exact_time.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include <kimera-vio/pipeline/Pipeline.h>
+#include "kimera_vio_ros/RosDataProviderInterface.hpp"
+
+
+using namespace message_filters::sync_policies;
+using namespace sensor_msgs::msg;
+
+ class KimeraVioNode : public VIO::RosDataProviderInterface{
+public:
+    KimeraVioNode(
+        const std::string & node_name,
+        const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+    ~KimeraVioNode();
+
+private:
+   VIO::FrameId frame_count_;
+
+  bool spin_outputs();
+  void stereo_cb(
+      const Image::SharedPtr left_msg,
+      const Image::SharedPtr right_msg);
+  void imu_cb(const Imu::SharedPtr imu_msg);
+  std::string timing_string();
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_stereo_;
+  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_imu_;
+  rclcpp::Subscription<Imu>::SharedPtr imu_sub_;
+
+  typedef ExactTime<Image, Image> ExactPolicy;
+  typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
+  std::shared_ptr<ExactSync> exact_sync_;
+  image_transport::SubscriberFilter left_sub_, right_sub_;
+
+  std::shared_ptr<message_filters::Subscriber<Image> > l_sub_;
+  std::shared_ptr<message_filters::Subscriber<Image> > r_sub_;
+
+  VIO::Pipeline vio_pipeline_;
+  std::future<bool> handle_pipeline_;
+  std::future<bool> handle_outputs_;
+  rclcpp::WallRate outputs_loop_rate_;
+
+  rclcpp::Time last_imu_timestamp_;
+  rclcpp::Time last_stereo_timestamp_;
+};