Add new driver sim

- realsense

bringup/config/params.realsense_driver.yaml

@@ -0,0 +1,16 @@
+realsense_driver:
+  ros__parameters:
+    use_sim_time: True
+    sim:
+      model: "cloi"
+      parts: "realsense"
+      manager_ip: "127.0.0.1"
+      manager_port: 25554
+
+    remapping_list: ['/tf', '/tf_static']
+
+    transform: [0.344000, 0.0, 1.014100, 0.0, 0.0, 0.0]
+
+    frame_id: "realsense_link"
+
+    module_list: ["rgb", "ir1", "ir2", "depth"]

bringup/launch/driver_sim.launch.py

@@ -35,6 +35,7 @@ def generate_launch_description():
         'micom_driver_sim',
         # 'camera_driver_sim',
         # 'depth_camera_driver_sim',
+        # 'realsense_driver_sim',
         # 'multi_camera_driver_sim'
         # 'gps_driver_sim',
     ]

bringup/launch/realsense_driver_sim.launch.py

@@ -0,0 +1,65 @@
+#
+# 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
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+from simdevice_bringup.common import get_modified_params_with_ns_and_remapping_list
+from simdevice_bringup.common import find_robot_name
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.actions import SetEnvironmentVariable
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+
+    _robot_name = LaunchConfiguration('robot_name')
+
+    # Get the launch directory
+    _pkg_name = "simdevice_bringup"
+
+    _config_dir = os.path.join(get_package_share_directory(_pkg_name), 'config')
+    config_params = os.path.join(_config_dir, 'params.realsense_driver.yaml')
+
+    _package_name = 'realsense_driver_sim'
+    _node_name = 'realsense_driver'
+
+    # modify config param with namespace
+    (_config_params, _remapping_list) = get_modified_params_with_ns_and_remapping_list(
+        config_params, _node_name)
+
+    start_realsense_driver_sim_cmd = Node(
+        package=_package_name,
+        node_executable=_package_name,
+        node_name=_node_name,
+        node_namespace=_robot_name,
+        remappings=_remapping_list,
+        parameters=[_config_params],
+        output='screen')
+
+    declare_launch_argument_rn = DeclareLaunchArgument(
+        'robot_name',
+        default_value=find_robot_name(),
+        description='It is robot name. same as `node namspace`')
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
+
+    # Create the launch description and populate
+    ld = launch.LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    ld.add_action(declare_launch_argument_rn)
+
+    ld.add_action(start_realsense_driver_sim_cmd)
+
+    return ld

bringup/package.xml

@@ -24,6 +24,7 @@
   <build_depend>camera_driver_sim</build_depend>
   <build_depend>multi_camera_driver_sim</build_depend>
   <build_depend>depth_camera_driver_sim</build_depend>
+  <build_depend>realsense_driver_sim</build_depend>
   <build_depend>elevator_system</build_depend>
 
   <exec_depend>launch_ros</exec_depend>
@@ -36,6 +37,7 @@
   <exec_depend>camera_driver_sim</exec_depend>
   <exec_depend>multi_camera_driver_sim</exec_depend>
   <exec_depend>depth_camera_driver_sim</exec_depend>
+  <exec_depend>realsense_driver_sim</exec_depend>
   <exec_depend>elevator_system</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>

realsense_driver_sim/CMakeLists.txt

@@ -0,0 +1,49 @@
+###############################################################################
+# Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.5)
+project(realsense_driver_sim)
+
+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 -fstack-protector -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(camera_info_manager REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(sim_bridge REQUIRED)
+find_package(driver_sim REQUIRED)
+
+################################################################################
+# Build
+################################################################################
+include_directories(include)
+
+add_executable(${PROJECT_NAME}
+  src/main.cpp
+  src/RealSenseDriverSim.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  sensor_msgs
+  camera_info_manager
+  image_transport
+  sim_bridge
+  driver_sim)
+
+################################################################################
+# Install
+################################################################################
+install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

realsense_driver_sim/README.md

@@ -0,0 +1 @@
+# Realsense Driver Sim

realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp

@@ -0,0 +1,65 @@
+/**
+ *  @file   RealSenseDriverSim.hpp
+ *  @date   2020-07-09
+ *  @author Sungkyu Kang
+ *  @author hyunseok Yang
+ *  @brief
+ *        ROS2 Depth Camera Driver class for simulator
+ *  @remark
+ *  @warning
+ *       LGE Advanced Robotics Laboratory
+ *         Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea
+ *         All Rights are Reserved.
+ */
+#ifndef _RealSenseDriverSim_H_
+#define _RealSenseDriverSim_H_
+
+#include "driver_sim/driver_sim.hpp"
+#include <image_transport/image_transport.h>
+#include <camera_info_manager/camera_info_manager.h>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <protobuf/image_stamped.pb.h>
+#include <protobuf/camerasensor.pb.h>
+
+class RealSenseDriverSim : public DriverSim
+{
+public:
+  RealSenseDriverSim();
+  virtual ~RealSenseDriverSim();
+
+private:
+  virtual void Initialize() override;
+  virtual void Deinitialize() override;
+  virtual void UpdateData(const int bridge_index) override;
+
+  void GetCameraSensorMessage(const int bridge_index);
+  void InitializeCameraInfoMessage(const std::string frame_id);
+
+private:
+  static const int max_modules = 4;
+
+  std::vector<std::thread> m_threads;
+
+  // key for connection
+  std::vector<std::string> m_hashKeySubs;
+
+  // buffer from simulation
+  std::vector<gazebo::msgs::ImageStamped> m_pbBuf;
+
+  // message for ROS2 communictaion
+  std::vector<sensor_msgs::msg::Image> msg_imgs;
+
+  // Camera sensor info buffer from simulator
+  gazebo::msgs::CameraSensor m_pbTmpBufCameraSensorInfo;
+
+  // Camera info publishers.
+  std::vector<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> pubCameraInfos;
+
+  // Camera info managers
+  std::vector<std::shared_ptr<camera_info_manager::CameraInfoManager>> cameraInfoManager;
+
+  // Image publisher
+  std::vector<image_transport::Publisher> pubImages;
+};
+
+#endif

realsense_driver_sim/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>realsense_driver_sim</name>
+  <version>1.0.0</version>
+  <description>virtual realsense driver for simulator</description>
+  <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
+  <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
+  <author email="sungkyu.kang@lge.com">Sungkyu Kang</author>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>sensor_msgs</depend>
+  <depend>camera_info_manager</depend>
+  <depend>image_transport</depend>
+
+  <build_depend>driver_sim</build_depend>
+  <build_depend>sim_bridge</build_depend>
+  <build_depend>rclcpp</build_depend>
+
+  <exec_depend>sim_bridge</exec_depend>
+  <exec_depend>driver_sim</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>