From 3f7b0bd4f97cda780037cc01ae4c4400e4746c70 Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Thu, 23 Jul 2020 16:48:12 +0900 Subject: [PATCH] Add new driver sim - realsense --- bringup/config/params.realsense_driver.yaml | 16 + bringup/launch/driver_sim.launch.py | 1 + bringup/launch/realsense_driver_sim.launch.py | 65 ++++ bringup/package.xml | 2 + realsense_driver_sim/CMakeLists.txt | 49 +++ realsense_driver_sim/README.md | 1 + .../RealSenseDriverSim.hpp | 65 ++++ realsense_driver_sim/package.xml | 28 ++ .../src/RealSenseDriverSim.cpp | 287 ++++++++++++++++++ realsense_driver_sim/src/main.cpp | 28 ++ 10 files changed, 542 insertions(+) create mode 100644 bringup/config/params.realsense_driver.yaml create mode 100644 bringup/launch/realsense_driver_sim.launch.py create mode 100644 realsense_driver_sim/CMakeLists.txt create mode 100644 realsense_driver_sim/README.md create mode 100644 realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp create mode 100644 realsense_driver_sim/package.xml create mode 100644 realsense_driver_sim/src/RealSenseDriverSim.cpp create mode 100644 realsense_driver_sim/src/main.cpp diff --git a/bringup/config/params.realsense_driver.yaml b/bringup/config/params.realsense_driver.yaml new file mode 100644 index 00000000..c41c4900 --- /dev/null +++ b/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"] diff --git a/bringup/launch/driver_sim.launch.py b/bringup/launch/driver_sim.launch.py index 9058e85a..09c4db14 100755 --- a/bringup/launch/driver_sim.launch.py +++ b/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', ] diff --git a/bringup/launch/realsense_driver_sim.launch.py b/bringup/launch/realsense_driver_sim.launch.py new file mode 100644 index 00000000..31f74b87 --- /dev/null +++ b/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 diff --git a/bringup/package.xml b/bringup/package.xml index a88dc2d7..de812bb0 100644 --- a/bringup/package.xml +++ b/bringup/package.xml @@ -24,6 +24,7 @@ camera_driver_sim multi_camera_driver_sim depth_camera_driver_sim + realsense_driver_sim elevator_system launch_ros @@ -36,6 +37,7 @@ camera_driver_sim multi_camera_driver_sim depth_camera_driver_sim + realsense_driver_sim elevator_system ament_lint_common diff --git a/realsense_driver_sim/CMakeLists.txt b/realsense_driver_sim/CMakeLists.txt new file mode 100644 index 00000000..325966c0 --- /dev/null +++ b/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() diff --git a/realsense_driver_sim/README.md b/realsense_driver_sim/README.md new file mode 100644 index 00000000..220702cf --- /dev/null +++ b/realsense_driver_sim/README.md @@ -0,0 +1 @@ +# Realsense Driver Sim \ No newline at end of file diff --git a/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp b/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp new file mode 100644 index 00000000..14948e0c --- /dev/null +++ b/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 +#include +#include +#include +#include + +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 m_threads; + + // key for connection + std::vector m_hashKeySubs; + + // buffer from simulation + std::vector m_pbBuf; + + // message for ROS2 communictaion + std::vector msg_imgs; + + // Camera sensor info buffer from simulator + gazebo::msgs::CameraSensor m_pbTmpBufCameraSensorInfo; + + // Camera info publishers. + std::vector::SharedPtr> pubCameraInfos; + + // Camera info managers + std::vector> cameraInfoManager; + + // Image publisher + std::vector pubImages; +}; + +#endif \ No newline at end of file diff --git a/realsense_driver_sim/package.xml b/realsense_driver_sim/package.xml new file mode 100644 index 00000000..201e67fe --- /dev/null +++ b/realsense_driver_sim/package.xml @@ -0,0 +1,28 @@ + + + realsense_driver_sim + 1.0.0 + virtual realsense driver for simulator + Sungkyu Kang + Hyunseok Yang + Sungkyu Kang + MIT + + ament_cmake + + sensor_msgs + camera_info_manager + image_transport + + driver_sim + sim_bridge + rclcpp + + sim_bridge + driver_sim + rclcpp + + + ament_cmake + + \ No newline at end of file diff --git a/realsense_driver_sim/src/RealSenseDriverSim.cpp b/realsense_driver_sim/src/RealSenseDriverSim.cpp new file mode 100644 index 00000000..f9e41599 --- /dev/null +++ b/realsense_driver_sim/src/RealSenseDriverSim.cpp @@ -0,0 +1,287 @@ +/** + * @file RealSenseDriverSim.cpp + * @date 2020-03-20 + * @author hyunseok Yang + * @author Sungkyu Kang + * @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. + */ +#include "realsense_driver_sim/RealSenseDriverSim.hpp" +#include "driver_sim/helper.h" +#include +#include +#include +#include + +using namespace std; +using namespace chrono_literals; +using namespace gazebo; + +RealSenseDriverSim::RealSenseDriverSim() + : DriverSim("realsense_driver_sim", max_modules * 2) +{ + Start(false); +} + +RealSenseDriverSim::~RealSenseDriverSim() +{ + DBG_SIM_INFO("Delete"); + Stop(); +} + +void RealSenseDriverSim::Initialize() +{ + string frame_id_; + vector transform_; + vector module_list_; + + get_parameter_or("frame_id", frame_id_, string("realsense_link")); + get_parameter_or("transform", transform_, vector({0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + + geometry_msgs::msg::TransformStamped camera_tf; + tf2::Quaternion fixed_rot; + fixed_rot.setRPY(transform_[3], transform_[4], transform_[5]); + + camera_tf.header.frame_id = "base_footprint"; + camera_tf.child_frame_id = frame_id_; + camera_tf.transform.translation.x = transform_[0]; + camera_tf.transform.translation.y = transform_[1]; + camera_tf.transform.translation.z = transform_[2]; + camera_tf.transform.rotation.x = fixed_rot.x(); + camera_tf.transform.rotation.y = fixed_rot.y(); + camera_tf.transform.rotation.z = fixed_rot.z(); + camera_tf.transform.rotation.w = fixed_rot.w(); + + AddStaticTf2(camera_tf); + + get_parameter("module_list", module_list_); + + while (module_list_.size() > max_modules) + { + DBG_SIM_ERR("the number of module is wrong"); + module_list_.pop_back(); + } + + int simBridgeCount = 0; + for (auto module : module_list_) + { + const auto topic_base_name_ = "realsense/" + module; + const auto hashKeySub = GetRobotName() + GetPartsName() + module; + m_hashKeySubs.push_back(hashKeySub); + + DBG_SIM_INFO("[CONFIG] topic_name:%s", topic_base_name_.c_str()); + DBG_SIM_INFO("[CONFIG] hash Key sub: %s", hashKeySub.c_str()); + + sensor_msgs::msg::Image msg_img; + msg_img.header.frame_id = module; + + msg_imgs.push_back(msg_img); + + auto pubImage = image_transport::create_publisher(GetNode(), topic_base_name_ + "/image_raw"); + pubImages.push_back(pubImage); + + auto pubCameraInfo = create_publisher(topic_base_name_ + "/camera_info", rclcpp::SensorDataQoS()); + pubCameraInfos.push_back(pubCameraInfo); + + GetSimBridge(simBridgeCount)->Connect(SimBridge::Mode::SUB, hashKeySub); + GetSimBridge(simBridgeCount + 1)->Connect(SimBridge::Mode::CLIENT, hashKeySub + "Info"); + + GetCameraSensorMessage(simBridgeCount + 1); + InitializeCameraInfoMessage(module); + + // m_threads.emplace_back([this]() { + m_threads.emplace_back(thread([=]() { + while (IsRunThread()) + { + UpdateData(simBridgeCount); + } + })); + + simBridgeCount += 2; + } +} + +void RealSenseDriverSim::Deinitialize() +{ + for (auto &thread : m_threads) + { + if (thread.joinable()) + { + thread.join(); + DBG_SIM_INFO("Thread finished"); + } + } + + for (auto pub : pubImages) + { + pub.shutdown(); + } + + DisconnectAllSimBridge(); +} + +void RealSenseDriverSim::GetCameraSensorMessage(const int bridge_index) +{ + msgs::Param request_msg; + request_msg.set_name("request_camera_info"); + + string serializedBuffer; + request_msg.SerializeToString(&serializedBuffer); + + auto simBridge = GetSimBridge(bridge_index); + simBridge->Send(serializedBuffer.data(), serializedBuffer.size()); + + void *pBuffer = nullptr; + int bufferLength = 0; + const auto succeeded = simBridge->Receive(&pBuffer, bufferLength); + + if (!succeeded || bufferLength < 0) + { + DBG_SIM_ERR("Faild to get camera info, length(%d)", bufferLength); + } + else + { + if (m_pbTmpBufCameraSensorInfo.ParseFromArray(pBuffer, bufferLength) == false) + { + DBG_SIM_ERR("Faild to Parsing Proto buffer pBuffer(%p) length(%d)", pBuffer, bufferLength); + } + } +} + +void RealSenseDriverSim::InitializeCameraInfoMessage(const string frame_id) +{ + sensor_msgs::msg::CameraInfo camera_info_msg; + + int width_ = m_pbTmpBufCameraSensorInfo.image_size().x(); + int height_ = m_pbTmpBufCameraSensorInfo.image_size().y(); + + // C parameters + auto cx_ = (static_cast(width_) + 1.0) / 2.0; + auto cy_ = (static_cast(height_) + 1.0) / 2.0; + + double hfov_ = m_pbTmpBufCameraSensorInfo.horizontal_fov(); + + auto computed_focal_length = (static_cast(width_)) / (2.0 * tan(hfov_ / 2.0)); + + // CameraInfo + camera_info_msg.header.frame_id = frame_id; + camera_info_msg.height = height_; + camera_info_msg.width = width_; + camera_info_msg.distortion_model = "plumb_bob"; + camera_info_msg.d.resize(5); + + const auto hack_baseline = 0.0f; + + // Get distortion from camera + double distortion_k1 = m_pbTmpBufCameraSensorInfo.distortion().k1(); + double distortion_k2 = m_pbTmpBufCameraSensorInfo.distortion().k2(); + double distortion_k3 = m_pbTmpBufCameraSensorInfo.distortion().k3(); + double distortion_t1 = m_pbTmpBufCameraSensorInfo.distortion().p1(); + double distortion_t2 = m_pbTmpBufCameraSensorInfo.distortion().p2(); + + // D = {k1, k2, t1, t2, k3}, as specified in: + // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + camera_info_msg.d[0] = distortion_k1; + camera_info_msg.d[1] = distortion_k2; + camera_info_msg.d[2] = distortion_t1; + camera_info_msg.d[3] = distortion_t2; + camera_info_msg.d[4] = distortion_k3; + + // Original camera matrix + camera_info_msg.k[0] = computed_focal_length; + camera_info_msg.k[1] = 0.0; + camera_info_msg.k[2] = cx_; + camera_info_msg.k[3] = 0.0; + camera_info_msg.k[4] = computed_focal_length; + camera_info_msg.k[5] = cy_; + camera_info_msg.k[6] = 0.0; + camera_info_msg.k[7] = 0.0; + camera_info_msg.k[8] = 1.0; + + // rectification + camera_info_msg.r[0] = 1.0; + camera_info_msg.r[1] = 0.0; + camera_info_msg.r[2] = 0.0; + camera_info_msg.r[3] = 0.0; + camera_info_msg.r[4] = 1.0; + camera_info_msg.r[5] = 0.0; + camera_info_msg.r[6] = 0.0; + camera_info_msg.r[7] = 0.0; + camera_info_msg.r[8] = 1.0; + + // camera_ projection matrix (same as camera_ matrix due + // to lack of distortion/rectification) (is this generated?) + camera_info_msg.p[0] = computed_focal_length; + camera_info_msg.p[1] = 0.0; + camera_info_msg.p[2] = cx_; + camera_info_msg.p[3] = -computed_focal_length * hack_baseline; + camera_info_msg.p[4] = 0.0; + camera_info_msg.p[5] = computed_focal_length; + camera_info_msg.p[6] = cy_; + camera_info_msg.p[7] = 0.0; + camera_info_msg.p[8] = 0.0; + camera_info_msg.p[9] = 0.0; + camera_info_msg.p[10] = 1.0; + camera_info_msg.p[11] = 0.0; + + // Initialize camera_info_manager + cameraInfoManager.push_back(std::make_shared(GetNode(), frame_id)); + cameraInfoManager.back()->setCameraInfo(camera_info_msg); +} + +void RealSenseDriverSim::UpdateData(const int bridge_index) +{ + const int module_index = bridge_index / 2; + void *pBuffer = nullptr; + int bufferLength = 0; + + auto simBridge = GetSimBridge(bridge_index); + const bool succeeded = simBridge->Receive(&pBuffer, bufferLength, false); + if (!succeeded || bufferLength < 0) + { + DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); + + // try reconnect1ion + if (IsRunThread()) + { + simBridge->Reconnect(SimBridge::Mode::SUB, m_hashKeySubs[module_index]); + } + + return; + } + + gazebo::msgs::ImageStamped pbBuf; + if (!pbBuf.ParseFromArray(pBuffer, bufferLength)) + { + DBG_SIM_ERR("Parsing error, size(%d)", bufferLength); + return; + } + + m_simTime = rclcpp::Time(pbBuf.time().sec(), pbBuf.time().nsec()); + + + msg_imgs[module_index].header.stamp = m_simTime; + + const auto encoding_arg = GetImageEncondingType(pbBuf.image().pixel_format()); + const uint32_t cols_arg = pbBuf.image().width(); + const uint32_t rows_arg = pbBuf.image().height(); + const uint32_t step_arg = pbBuf.image().step(); + + // Copy from src to image_msg + sensor_msgs::fillImage(msg_imgs[module_index], encoding_arg, rows_arg, cols_arg, step_arg, + reinterpret_cast(pbBuf.image().data().data())); + + pubImages[module_index].publish(msg_imgs[module_index]); + + // Publish camera info + auto camera_info_msg = cameraInfoManager[module_index]->getCameraInfo(); + camera_info_msg.header.stamp = m_simTime; + + pubCameraInfos[module_index]->publish(camera_info_msg); +} \ No newline at end of file diff --git a/realsense_driver_sim/src/main.cpp b/realsense_driver_sim/src/main.cpp new file mode 100644 index 00000000..6c4ff13f --- /dev/null +++ b/realsense_driver_sim/src/main.cpp @@ -0,0 +1,28 @@ +/** + * @file main.cpp + * @date 2020-05-13 + * @author Sungkyu Kang + * @brief + * ROS2 Camera Driver class for simulator + * @remark + * @warning + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ +#include "realsense_driver_sim/RealSenseDriverSim.hpp" + +using namespace std::literals; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::sleep_for(100ms); + + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +}