ROS 2 RGB-D reader

GUI/CMakeLists.txt

@@ -42,8 +42,9 @@ if(ROSNODE)
     if(HASROS1)
         find_package(roscpp REQUIRED)
         find_package(eigen_conversions REQUIRED)
-        add_compile_options(-DROSREADER -DROSUI -DROSSTATE)
+        add_compile_options(-DROSUI -DROSSTATE)
     endif()
+    add_compile_options(-DROSREADER)
     find_package(image_transport REQUIRED)
     find_package(tf2_ros REQUIRED)
     find_package(cob_srvs REQUIRED)
@@ -148,6 +149,12 @@ if(ROSNODE)
         ${eigen_conversions_LIBRARIES}
         ${cob_srvs_LIBRARIES}
     )
+if(HASROS2)
+    target_link_libraries(MultiMotionFusionTools
+        image_geometry::image_geometry
+        image_transport::image_transport
+    )
+endif()
     # to find "gSLICr.h" and "super_point_inference.hpp"
     target_link_libraries(MultiMotionFusionTools gSLICr_lib super_point_inference)
     add_definitions(-DROSNODE)
@@ -167,6 +174,7 @@ if(ROSBAG OR ROSNODE)
 if(HASROS2)
     target_link_libraries(MultiMotionFusionTools
         image_geometry::image_geometry
+        cv_bridge::cv_bridge
     )
 endif()
     add_definitions(-DROSCOMMON)

GUI/MainController.cpp

@@ -248,12 +248,32 @@ MainController::MainController(int argc, char* argv[])
     // instantiate MultiMotionFusion node
 #ifdef ROS1
     ros::init(argc, argv, "MMF");
+    executor = std::make_unique<ros::AsyncSpinner>(0);
+    executor->start();
+#elif defined(ROS2)
+    const std::vector<std::string> args_non_ros = rclcpp::init_and_remove_ros_arguments(argc, argv);
+    // reset argv without the parsed ROS args
+    for (int i = 0; i < argc; i++) {
+        memset(argv[i], 0, strlen(argv[i]));
+    }
+    argc = args_non_ros.size();
+    for (size_t i=0; i<args_non_ros.size(); i++) {
+      strcpy(argv[i], args_non_ros[i].c_str());
+    }
+
+    executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
+    spinner = std::thread([this](){ executor->spin(); });
 #endif
 #ifdef ROSREADER
     // read RGB-D data
     if (!logReader) {
-      logReader = std::make_unique<RosNodeReader>(15, Parse::get().arg(argc, argv, "-f", empty) > -1, target_dim);
-      logReaderReady = true;
+      try {
+        logReader = std::make_unique<RosNodeReader>(15, Parse::get().arg(argc, argv, "-f", empty) > -1, target_dim);
+        logReaderReady = true;
+      } catch (const std::runtime_error& e) {
+        logReader = nullptr;
+        logReaderReady = false;
+      }
     }
 #endif
 #ifdef ROSSTATE
@@ -263,6 +283,14 @@ MainController::MainController(int argc, char* argv[])
 #endif
 #ifdef ROSUI
     ui_control = std::make_unique<RosInterface>(&gui, &mmf);
+#endif
+#if defined(ROS2)
+    // add nodes to executor
+#ifdef ROSREADER
+    if (logReader) {
+      executor->add_node(dynamic_cast<RosNodeReader*>(logReader.get())->n);
+    }
+#endif
 #endif
   }
 #endif
@@ -413,6 +441,14 @@ MainController::MainController(int argc, char* argv[])
 }
 
 MainController::~MainController() {
+#ifdef ROSNODE
+#ifdef ROS1
+  executor->stop();
+#elif defined(ROS2)
+  executor->cancel();
+  spinner.join();
+#endif
+#endif
   if (mmf) {
     delete mmf;
   }

GUI/MainController.h

@@ -31,6 +31,10 @@
 #ifdef ROSUI
 #include "Tools/RosInterface.hpp"
 #endif
+#if defined(ROS2)
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <thread>
+#endif
 
 class MainController {
  public:
@@ -75,6 +79,15 @@ class MainController {
   std::unique_ptr<RosInterface> ui_control;
 #endif
 
+#ifdef ROSNODE
+#ifdef ROS1
+  std::unique_ptr<ros::AsyncSpinner> executor;
+#elif defined(ROS2)
+  std::unique_ptr<rclcpp::executors::MultiThreadedExecutor> executor;
+  std::thread spinner;
+#endif
+#endif
+
   float confGlobalInit, confObjectInit, icpErrThresh, covThresh, photoThresh, fernThresh;
 
   int timeDelta, icpCountThresh, start, end, preallocatedModelsCount;

GUI/Tools/RosNodeReader.cpp

@@ -1,9 +1,22 @@
 #ifdef ROSREADER
 
 #include "RosNodeReader.hpp"
-#include <sensor_msgs/CameraInfo.h>
 #include <cv_bridge/cv_bridge.h>
-#include <tf2_eigen/tf2_eigen.h>
+
+#if defined(ROS1)
+    #include <tf2_eigen/tf2_eigen.h>
+    #include <sensor_msgs/CameraInfo.h>
+    #include <std_msgs/Header.h>
+    using namespace sensor_msgs;
+    using namespace std_msgs;
+#elif defined(ROS2)
+    #include <tf2_eigen/tf2_eigen.hpp>
+    #include <rclcpp/wait_for_message.hpp>
+    #include <sensor_msgs/msg/camera_info.hpp>
+    #include <std_msgs/msg/header.hpp>
+    using namespace sensor_msgs::msg;
+    using namespace std_msgs::msg;
+#endif
 
 
 RosNodeReader::RosNodeReader(const uint32_t synchroniser_queue_size,
@@ -12,21 +25,39 @@ RosNodeReader::RosNodeReader(const uint32_t synchroniser_queue_size,
   LogReader(std::string(), flipColors),
   frame_gt_camera(frame_gt_camera)
 {
+#if defined(ROS1)
   n = std::make_unique<ros::NodeHandle>();
   it = std::make_unique<image_transport::ImageTransport>(*n);
+#elif defined(ROS2)
+  n = std::make_shared<rclcpp::Node>("MMF");
+  const std::string transport = n->declare_parameter("image_transport", "raw");
+#endif
 
   tf_listener = std::make_unique<tf2_ros::TransformListener>(tf_buffer);
 
-  sub_colour.subscribe(*it, ros::names::resolve("colour"), 1);
-  sub_depth.subscribe(*it, ros::names::resolve("depth"), 1);
+#if defined(ROS1)
+  sub_colour.subscribe(*it, resolve("colour"), 1);
+  sub_depth.subscribe(*it, resolve("depth"), 1);
+#elif defined(ROS2)
+  sub_colour.subscribe(n.get(), resolve("colour"), transport);
+  sub_depth.subscribe(n.get(), resolve("depth"), transport);
+#endif
 
   sync = std::make_unique<message_filters::Synchronizer<ApproximateTimePolicy>>(ApproximateTimePolicy(synchroniser_queue_size));
   sync->connectInput(sub_colour, sub_depth);
   sync->registerCallback(&RosNodeReader::on_rgbd, this);
 
   // wait for single CameraInfo message to get intrinsics
-  std::cout << "waiting for 'sensor_msgs/CameraInfo' message on '" + ros::names::resolve("camera_info") + "'" << std::endl;
-  sensor_msgs::CameraInfo::ConstPtr ci = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", *n);
+  std::cout << "waiting for 'sensor_msgs/CameraInfo' message on '" + resolve("camera_info") + "'" << std::endl;
+
+#if defined(ROS1)
+  CameraInfo::ConstPtr ci = ros::topic::waitForMessage<CameraInfo>("camera_info", *n);
+#elif defined(ROS2)
+  CameraInfo::Ptr ci = std::make_shared<CameraInfo>();
+  if(!rclcpp::wait_for_message(*ci, n, "camera_info")) {
+    throw std::runtime_error("error while waiting for message");
+  }
+#endif
 
   image_crop_target = ImageCropTarget(ci, target_dimensions);
 
@@ -35,19 +66,16 @@ RosNodeReader::RosNodeReader(const uint32_t synchroniser_queue_size,
   numPixels = width * height;
 
   ref_pose.matrix().array() = 0;
-
-  spinner = std::make_unique<ros::AsyncSpinner>(1);
-  spinner->start();
 }
 
-RosNodeReader::~RosNodeReader() {
-  spinner->stop();
-}
-
-void RosNodeReader::on_rgbd(const sensor_msgs::Image::ConstPtr& msg_colour, const sensor_msgs::Image::ConstPtr& msg_depth) {
+void RosNodeReader::on_rgbd(const Image::ConstPtr& msg_colour, const Image::ConstPtr& msg_depth) {
   mutex.lock();
-  const std_msgs::Header hdr_colour = msg_colour->header;
+  const Header hdr_colour = msg_colour->header;
+#if defined(ROS1)
   data.timestamp = int64_t(hdr_colour.stamp.toNSec());
+#elif defined(ROS2)
+  data.timestamp = int64_t(hdr_colour.stamp.sec * 1e9 + hdr_colour.stamp.nanosec);
+#endif
   data.rgb = cv_bridge::toCvCopy(msg_colour, "rgb8")->image;
 
   data.depth = cv_bridge::toCvCopy(msg_depth)->image;
@@ -105,8 +133,12 @@ FrameData RosNodeReader::getFrameData() {
 
 Eigen::Matrix4f RosNodeReader::getIncrementalTransformation(uint64_t timestamp) {
   // camera pose at requested time with respect to root frame
+#if defined(ROS1)
   ros::Time time;
   time.fromNSec(timestamp);
+#elif defined(ROS2)
+  tf2::TimePoint time{std::chrono::nanoseconds(timestamp)};
+#endif
   Eigen::Isometry3d pose;
   try {
     pose = tf2::transformToEigen(tf_buffer.lookupTransform(frame_gt_root, frame_gt_camera, time));
@@ -124,4 +156,12 @@ Eigen::Matrix4f RosNodeReader::getIncrementalTransformation(uint64_t timestamp)
   return (ref_pose.inverse() * pose).matrix().cast<float>();
 }
 
+std::string RosNodeReader::resolve(const std::string &topic) {
+#if defined(ROS1)
+  return ros::names::resolve(topic);
+#elif defined(ROS2)
+  return this->n->get_node_base_interface()->resolve_topic_or_service_name(topic, false);
+#endif
+}
+
 #endif

GUI/Tools/RosNodeReader.hpp

@@ -2,25 +2,32 @@
 
 #pragma once
 #include "LogReader.h"
-#include <sensor_msgs/Image.h>
-#include <ros/ros.h>
-#include <image_transport/subscriber_filter.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include "ros_common.hpp"
 #include <Utils/GroundTruthOdometryInterface.hpp>
 #include <Eigen/Geometry>
 #include <tf2_ros/transform_listener.h>
 
+#if defined(ROS1)
+    #include <ros/ros.h>
+    #include <sensor_msgs/Image.h>
+    using namespace sensor_msgs;
+    #include <image_transport/subscriber_filter.h>
+#elif defined(ROS2)
+    #include <rclcpp/rclcpp.hpp>
+    #include <sensor_msgs/msg/image.h>
+    using namespace sensor_msgs::msg;
+    #include <image_transport/subscriber_filter.hpp>
+#endif
+
 
 class RosNodeReader : public LogReader, public GroundTruthOdometryInterface {
 public:
   RosNodeReader(const uint32_t synchroniser_queue_size,
                 const bool flipColors = false, const cv::Size &target_dimensions = {},
                 const std::string frame_gt_camera = {});
 
-  ~RosNodeReader();
-
-  void on_rgbd(const sensor_msgs::ImageConstPtr& msg_colour, const sensor_msgs::ImageConstPtr& msg_depth);
+  void on_rgbd(const Image::ConstPtr& msg_colour, const Image::ConstPtr& msg_depth);
 
   void getNext() override;
 
@@ -42,13 +49,18 @@ class RosNodeReader : public LogReader, public GroundTruthOdometryInterface {
 
   Eigen::Matrix4f getIncrementalTransformation(uint64_t timestamp) override;
 
+#if defined(ROS2)
+  rclcpp::Node::SharedPtr n;
+#endif
+
 private:
-  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ApproximateTimePolicy;
+  typedef message_filters::sync_policies::ApproximateTime<Image, Image> ApproximateTimePolicy;
 
   // node
+#if defined(ROS1)
   std::unique_ptr<ros::NodeHandle> n;
+#endif
   std::unique_ptr<image_transport::ImageTransport> it;
-  std::unique_ptr<ros::AsyncSpinner> spinner;
 
   // ground truth camera poses in root frame
   std::string frame_gt_root;
@@ -67,6 +79,8 @@ class RosNodeReader : public LogReader, public GroundTruthOdometryInterface {
 
   std::mutex mutex;
   FrameData data;
+
+  std::string resolve(const std::string &topic);
 };
 
 #endif