Add first source code for realsense

.gitignore

@@ -1,34 +1,41 @@
-devel/
+#
+# Copyright (c) 2020 LG Electronics Inc.
+#
+# SPDX-License-Identifier: MIT
+#
+
 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
+install/
+tags
+
+# vscide stuff
+.vscode
+*.code-workspace
 
 # eclipse stuff
 .project
 .cproject
 
+# Swap
+[._]*.s[a-v][a-z]
+!*.svg  # comment out if you don't need vector files
+[._]*.sw[a-p]
+[._]s[a-rt-v][a-z]
+[._]ss[a-gi-z]
+[._]sw[a-p]
+
+# Session
+Session.vim
+Sessionx.vim
+
+# Temporary
+.netrwhist
+*~
+
+# Emacs
+.#*
+
 # qcreator stuff
 CMakeLists.txt.user
 
@@ -38,14 +45,11 @@ srv/_*.py
 qtcreator-*
 *.user
 
-/planning/cfg
-/planning/docs
-/planning/src
-
-*~
+# Auto-generated tag files
+tags
 
-# Emacs
-.#*
+# Persistent undo
+[._]*.un~
 
-# Catkin custom files
-CATKIN_IGNORE
+# COLCON custom files
+COLCON_IGNORE

CMakeLists.txt

@@ -0,0 +1,51 @@
+###############################################################################
+# Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.5)
+project(realsense_adaptor)
+
+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 -O2)
+endif()
+
+################################################################################
+# Find colcon packages and libraries for colcon and system dependencies
+################################################################################
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(realsense_adaptor_msgs REQUIRED)
+
+
+################################################################################
+# Build
+################################################################################
+include_directories(include)
+
+add_executable(${PROJECT_NAME}
+  src/realsense_adaptor.cpp
+  src/main.cpp)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  sensor_msgs
+  realsense_adaptor_msgs
+  message_filters)
+
+
+################################################################################
+# Install
+################################################################################
+install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+
+################################################################################
+# Test
+################################################################################
+
+ament_package()

README.md

@@ -1,2 +1,5 @@
 # realsense_adaptor
+
 synchronize the image data published from realsense node
+
+realsense_adaptor_msgs is required for this package

config/realsense_adaptor.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    queue_size: 5
+
+    realsense_devices: ["realsense_front", "realsense_ground"]
+
+    realsense_color_info_topic: "color/camera_info"
+    realsense_color_topic: "color/image_raw"
+    realsense_depth_info_topic: "depth/camera_info"
+    realsense_depth_topic: "depth/image_raw"
+
+    realsense_adaptor_ouput_topic: "synced_image"

include/realsense_adaptor/realsense_adaptor.hpp

@@ -0,0 +1,66 @@
+/**
+ *  @file   realsense_adaptor.hpp
+ *  @date   2020-09-17
+ *  @author hyunseok Yang
+ *  @brief  ROS2 realsense adaptor utility
+ *  @remark
+ *  @warning
+ *       LGE Advanced Robotics Laboratory
+ *         Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea
+ *         All Rights are Reserved.
+ */
+
+#ifndef _REALSENSE_ADAPTOR_H_
+#define _REALSENSE_ADAPTOR_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <realsense_adaptor_msgs/msg/synced_image.hpp>
+
+#include <vector>
+
+class RealsenseAdaptor : public rclcpp::Node
+{
+public:
+  explicit RealsenseAdaptor(const std::string node_name);
+  ~RealsenseAdaptor();
+
+private:
+  struct CameraSet
+  {
+    void create(rclcpp::Node::SharedPtr node, std::string infoTopic, std::string dataTopic);
+
+    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr infoSub;
+
+    sensor_msgs::msg::CameraInfo infoMessage;
+    message_filters::Subscriber<sensor_msgs::msg::Image> imageSub;
+  };
+
+  struct AdaptorSet
+  {
+    std::string moduleName;
+
+    CameraSet color;
+    CameraSet depth;
+
+    // TODO: To be removed(?)
+    // message_filters::Subscriber<realsense_camera_msgs::msg::DeviceInfo> deviceSub;
+
+    std::unique_ptr<message_filters::TimeSynchronizer<sensor_msgs::msg::Image, sensor_msgs::msg::Image>> sync;
+
+    realsense_adaptor_msgs::msg::SyncedImage pubMessage;
+    rclcpp::Publisher<realsense_adaptor_msgs::msg::SyncedImage>::SharedPtr pub;
+
+    void callback(sensor_msgs::msg::Image::ConstSharedPtr, sensor_msgs::msg::Image::ConstSharedPtr);
+  };
+
+  std::vector<std::shared_ptr<AdaptorSet>> adaptorSetList_;
+
+private:
+  static void SetCameraInfo2CameraIntrinsic(const sensor_msgs::msg::CameraInfo* const, realsense_adaptor_msgs::msg::CameraIntrinsic&);
+};
+
+#endif

launch/realsense_adaptor.launch.py

@@ -0,0 +1,41 @@
+#
+# LGE Advanced Robotics Laboratory
+# Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea
+# All Rights are Reserved.
+#
+# SPDX-License-Identifier: MIT
+#
+
+import os
+import launch.actions
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+
+    # Get the launch directory
+    _package_name = "realsense_adaptor"
+    config_file_path = os.path.join(get_package_share_directory(_package_name), 'config', "realsense_adaptor.yaml")
+
+    _namespace = LaunchConfiguration('robot_name')
+
+    start_node_cmd = Node(
+        package=_package_name,
+        node_executable=_package_name,
+        node_name=_package_name,
+        node_namespace=_namespace,
+        parameters=[config_file_path],
+        output='screen')
+
+    declare_launch_argument = DeclareLaunchArgument('robot_name', default_value='', description='')
+
+    # Create the launch description and populate
+    ld = launch.LaunchDescription()
+
+    ld.add_action(declare_launch_argument)
+    ld.add_action(start_node_cmd)
+
+    return ld

package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>realsense_adaptor</name>
+  <version>0.1.0</version>
+  <description>synchronize time for each channel's data from realsense module</description>
+  <maintainer email="[email protected]">Hyunseok Yang</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>realsense_adaptor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/main.cpp

@@ -0,0 +1,24 @@
+/**
+ *  @file   main.cpp
+ *  @date   2020-09-17
+ *  @author Hyunseok Yang
+ *  @brief  ROS2 adaptor node that controls realsense data
+ *  @remark
+ *  @copyright
+ *      LGE Advanced Robotics Laboratory
+ *      Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea
+ *      All Rights are Reserved.
+ *
+ *      SPDX-License-Identifier: MIT
+ */
+
+#include "realsense_adaptor/realsense_adaptor.hpp"
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<RealsenseAdaptor>("realsense_adaptor");
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}