From 8509ccfd67a1552839cefb89153078c9586b6b46 Mon Sep 17 00:00:00 2001 From: Joe Dong Date: Tue, 5 Sep 2023 17:59:19 +0800 Subject: [PATCH] add gemini2 XL launch file --- orbbec_camera/CMakeLists.txt | 96 +++++------ .../include/orbbec_camera/gst_decoder.h | 28 ---- .../include/orbbec_camera/ob_camera_node.h | 37 +++-- .../include/orbbec_camera/rk_mpp_decoder.h | 8 +- orbbec_camera/include/orbbec_camera/utils.h | 2 +- orbbec_camera/launch/astra.launch.py | 3 - orbbec_camera/launch/astra2.launch.py | 3 - orbbec_camera/launch/astra_adv.launch.py | 3 - .../launch/astra_embedded_s.launch.py | 3 - .../launch/astra_stereo_u3.launch.py | 3 - orbbec_camera/launch/dabai.launch.py | 3 - orbbec_camera/launch/deeya.launch.py | 3 - orbbec_camera/launch/femto.launch.py | 3 - orbbec_camera/launch/femto_mega.launch.py | 3 - orbbec_camera/launch/gemini2.launch.py | 3 - orbbec_camera/launch/gemini2L.launch.py | 3 - orbbec_camera/launch/gemini2XL.launch.py | 112 +++++++++++++ orbbec_camera/launch/gemini_e.launch.py | 3 - orbbec_camera/launch/ob_camera.launch.py | 3 - orbbec_camera/src/gst_decoder.cpp | 155 ------------------ orbbec_camera/src/ob_camera_node.cpp | 78 ++++----- orbbec_camera/src/ob_camera_node_driver.cpp | 7 +- orbbec_camera/src/rk_mpp_decoder.cpp | 21 ++- orbbec_camera/src/utils.cpp | 30 ++-- 24 files changed, 248 insertions(+), 365 deletions(-) delete mode 100644 orbbec_camera/include/orbbec_camera/gst_decoder.h create mode 100644 orbbec_camera/launch/gemini2XL.launch.py delete mode 100644 orbbec_camera/src/gst_decoder.cpp diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index a7e5e057..06761cdd 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -9,10 +9,9 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g") set(CMAKE_BUILD_TYPE "Release") option(USE_RK_HW_DECODER "Use Rockchip hardware decoder" OFF) -option(USE_GST_HW_DECODER "Use Gstreamer hardware decoder" OFF) if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Werror) + add_compile_options(-Wall -Wextra -Werror -Wpedantic) endif () # find dependencies @@ -56,23 +55,14 @@ if (USE_RK_HW_DECODER) if (NOT RK_MPP_FOUND) message(FATAL_ERROR "rockchip_mpp is not found") endif () - pkg_search_module(RGA REQUIRED librga) + pkg_search_module(RGA librga) if (NOT RGA_FOUND) - message(FATAL_ERROR "librga is not found") + add_definitions(-DUSE_LIBYUV) + message("librga is not found, use libyuv instead") endif () endif () -if (USE_GST_HW_DECODER) - find_package(PkgConfig REQUIRED) - pkg_search_module(GST REQUIRED gstreamer-1.0) - if (NOT GST_FOUND) - message(FATAL_ERROR "gstreamer-1.0 is not found") - endif () - pkg_search_module(GST_APP REQUIRED gstreamer-app-1.0) - if (NOT GST_APP_FOUND) - message(FATAL_ERROR "gstreamer-app-1.0 is not found") - endif () -endif () + execute_process(COMMAND uname -m OUTPUT_VARIABLE MACHINES) execute_process(COMMAND getconf LONG_BIT OUTPUT_VARIABLE MACHINES_BIT) message(STATUS "ORRBEC Machine : ${MACHINES}") @@ -86,33 +76,32 @@ elseif ((${MACHINES} MATCHES "aarch64") AND (${MACHINES_BIT} MATCHES "64")) set(HOST_PLATFORM "arm64") endif () -set(ORBBEC_LIBS ${CMAKE_CURRENT_SOURCE_DIR}/SDK/lib/${HOST_PLATFORM}) +set(ORBBEC_LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/lib/${HOST_PLATFORM}) set(ORBBEC_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/SDK/include/) -set(common_include_dirs +set(CMAKE_BUILD_RPATH "${CMAKE_BUILD_RPATH}:${ORBBEC_LIBS_DIR}") +set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH}:${ORBBEC_LIBS_DIR}") + +set(COMMON_INCLUDE_DIRS $ $ $ ${ORBBEC_INCLUDE_DIR} ${OpenCV_INCLUDED_DIRS} ${GLOG_INCLUDED_DIRS} - ${RK_MPP_INCLUDE_DIRS} - ${RGA_INCLUDE_DIRS} - ${GST_INCLUDE_DIRS} - ${GST_APP_INCLUDE_DIRS} ) -set(common_libraries +set(COMMON_LIBRARIES ${ORBBEC_SDK_LIBRARIES} ${OpenCV_LIBS} Eigen3::Eigen ${GLOG_LIBRARIES} -lOrbbecSDK - -L${ORBBEC_LIBS} + -L${ORBBEC_LIBS_DIR} ${RK_MPP_LIBRARIES} ) -set(source_files +set(SOURCE_FILES src/d2c_viewer.cpp src/dynamic_params.cpp src/ob_camera_node_driver.cpp @@ -126,49 +115,38 @@ set(source_files if (USE_RK_HW_DECODER) add_definitions(-DUSE_RK_HW_DECODER) - list(APPEND source_files src/rk_mpp_decoder.cpp) -endif () - -if (USE_GST_HW_DECODER) - add_definitions(-DUSE_GST_HW_DECODER) - list(APPEND source_files src/gst_decoder.cpp) -endif () - -macro(add_orbbec_executable target source) - add_executable(${target} ${source}) - target_include_directories(${target} PUBLIC ${common_include_dirs}) - target_link_libraries(${target} ${common_libraries} ${PROJECT_NAME}) - ament_target_dependencies(${target} ${dependencies}) - if (USE_RK_HW_DECODER) - target_link_libraries(${target} - ${RGA_LIBRARIES} - ) - elseif (USE_GST_HW_DECODER) - target_link_libraries(${target} - ${GST_LIBRARIES} - ${GST_APP_LIBRARIES} + list(APPEND SOURCE_FILES src/rk_mpp_decoder.cpp) + list(APPEND COMMON_INCLUDE_DIRS + ${RK_MPP_INCLUDE_DIRS} + ${RGA_INCLUDE_DIRS} + ) + list(APPEND COMMON_LIBRARIES + ${RGA_LIBRARIES} + ${RK_MPP_LIBRARIES} + ) + if (NOT RGA_FOUND) + list(APPEND COMMON_LIBRARIES + -lyuv ) endif () +endif () + +macro(add_orbbec_executable TARGET SOURCE) + add_executable(${TARGET} ${SOURCE}) + target_include_directories(${TARGET} PUBLIC ${COMMON_INCLUDE_DIRS}) + target_link_libraries(${TARGET} ${COMMON_LIBRARIES} ${PROJECT_NAME}) + ament_target_dependencies(${TARGET} ${dependencies}) endmacro() # Define library and nodes add_library(${PROJECT_NAME} SHARED - ${source_files} + ${SOURCE_FILES} ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_include_directories(${PROJECT_NAME} PUBLIC ${common_include_dirs}) -target_link_libraries(${PROJECT_NAME} ${common_libraries}) -if (USE_RK_HW_DECODER) - target_link_libraries(${PROJECT_NAME} - ${RGA_LIBRARIES} - ) -elseif (USE_GST_HW_DECODER) - target_link_libraries(${PROJECT_NAME} - ${GST_LIBRARIES} - ${GST_APP_LIBRARIES} - ) -endif () +target_include_directories(${PROJECT_NAME} PUBLIC ${COMMON_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${COMMON_LIBRARIES}) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "orbbec_camera::OBCameraNodeDriver" @@ -191,7 +169,7 @@ install(DIRECTORY include/ DESTINATION include) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/) install(DIRECTORY ${ORBBEC_INCLUDE_DIR} DESTINATION include) -install(DIRECTORY ${ORBBEC_LIBS}/ DESTINATION lib/ FILES_MATCHING PATTERN "*.so*") +install(DIRECTORY ${ORBBEC_LIBS_DIR}/ DESTINATION lib/ FILES_MATCHING PATTERN "*.so*") install(TARGETS list_devices_node ob_cleanup_shm_node list_depth_work_mode_node diff --git a/orbbec_camera/include/orbbec_camera/gst_decoder.h b/orbbec_camera/include/orbbec_camera/gst_decoder.h deleted file mode 100644 index 825eaf22..00000000 --- a/orbbec_camera/include/orbbec_camera/gst_decoder.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once -#include "mjpeg_decoder.h" -#include -namespace orbbec_camera { - -class GstreamerMjpegDecoder : public MjpegDecoder { - public: - GstreamerMjpegDecoder(int width, int height, std::string jpeg_decoder, std::string video_convert, - std::string jpeg_parse); - ~GstreamerMjpegDecoder() override; - - bool decode(const std::shared_ptr& frame, uint8_t* dest) override; - - private: - std::string jpeg_decoder_; - std::string video_convert_; - std::string jpeg_parse_; - guint buffer_size_ = 0; - GstBufferPool* buffer_pool_ = nullptr; - GstElement* pipeline_ = nullptr; - GstElement* appsrc_ = nullptr; - GstElement* jpegparse_ = nullptr; - GstElement* jpegdec_ = nullptr; - GstElement* videoconvert_ = nullptr; - GstElement* appsink_ = nullptr; -}; - -} // namespace orbbec_camera \ No newline at end of file diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 302da3b4..6d9703a3 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -57,11 +57,6 @@ #include "orbbec_camera/d2c_viewer.h" #include "magic_enum/magic_enum.hpp" #include "mjpeg_decoder.h" -#if defined(USE_RK_HW_DECODER) -#include "orbbec_camera/rk_mpp_decoder.h" -#elif defined(USE_GST_HW_DECODER) -#include "orbbec_camera/gst_decoder.h" -#endif #define STREAM_NAME(sip) \ (static_cast(std::ostringstream() \ @@ -102,16 +97,26 @@ typedef std::pair stream_index_pair; const stream_index_pair COLOR{OB_STREAM_COLOR, 0}; const stream_index_pair DEPTH{OB_STREAM_DEPTH, 0}; const stream_index_pair INFRA0{OB_STREAM_IR, 0}; -const stream_index_pair INFRA1{OB_STREAM_IR, 1}; -const stream_index_pair INFRA2{OB_STREAM_IR, 2}; +const stream_index_pair INFRA1{OB_STREAM_IR_LEFT, 0}; +const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0}; const stream_index_pair GYRO{OB_STREAM_GYRO, 0}; const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0}; -const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, COLOR}; +const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, COLOR, INFRA1, INFRA2}; const std::vector HID_STREAMS = {GYRO, ACCEL}; +const std::map STREAM_TYPE_TO_FRAME_TYPE = { + {OB_STREAM_COLOR, OB_FRAME_COLOR}, + {OB_STREAM_DEPTH, OB_FRAME_DEPTH}, + {OB_STREAM_IR, OB_FRAME_IR}, + {OB_STREAM_IR_LEFT, OB_FRAME_IR_LEFT}, + {OB_STREAM_IR_RIGHT, OB_FRAME_IR_RIGHT}, + {OB_STREAM_GYRO, OB_FRAME_GYRO}, + {OB_STREAM_ACCEL, OB_FRAME_ACCEL}, +}; + class OBCameraNode { public: OBCameraNode(rclcpp::Node* node, std::shared_ptr device, @@ -359,8 +364,6 @@ class OBCameraNode { ob::PointCloudFilter point_cloud_filter_; sensor_msgs::msg::PointCloud2 point_cloud_msg_; - rclcpp::Publisher::SharedPtr extrinsics_publisher_; - bool enable_publish_extrinsic_ = false; orbbec_camera_msgs::msg::DeviceInfo device_info_; std::string point_cloud_qos_; std::vector static_tf_msgs_; @@ -388,12 +391,13 @@ class OBCameraNode { // Only for Gemini2 device bool enable_hardware_d2d_ = true; std::string depth_work_mode_; - OBSyncMode sync_mode_ = OBSyncMode::OB_SYNC_MODE_CLOSE; + OBMultiDeviceSyncMode sync_mode_ = OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN; std::string sync_mode_str_; - int ir_trigger_signal_in_delay_ = 0; - int rgb_trigger_signal_in_delay_ = 0; - int device_trigger_signal_out_delay_ = 0; - bool sync_signal_trigger_out_ = false; + int depth_delay_us_ = 0; + int color_delay_us_ = 0; + int trigger2image_delay_us_ = 0; + int trigger_signal_output_delay_us_ = 0; + bool trigger_signal_output_enabled_ = false; std::string depth_precision_str_; OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8; // IMU @@ -409,8 +413,5 @@ class OBCameraNode { // mjpeg decoder std::shared_ptr mjpeg_decoder_ = nullptr; uint8_t* rgb_buffer_ = nullptr; - std::string jpeg_decoder_ = "avdec_mjpeg"; // avdec_mjpeg, mppjpegdec, nvjpegdec, jpegdec - std::string jpeg_parse_ = "jpegparse"; - std::string video_convert_ = "videoconvert"; // videoconvert, nvvidconv }; } // namespace orbbec_camera diff --git a/orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h b/orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h index 76ed755b..7a648fee 100644 --- a/orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h +++ b/orbbec_camera/include/orbbec_camera/rk_mpp_decoder.h @@ -1,7 +1,6 @@ #pragma once #include "mjpeg_decoder.h" -#include #include #include #include @@ -10,6 +9,11 @@ #include #include #include +#if defined(USE_LIBYUV) +#include +#else +#include +#endif #define MPP_ALIGN(x, a) (((x) + (a)-1) & ~((a)-1)) namespace orbbec_camera { @@ -36,7 +40,7 @@ class RKMjpegDecoder : public MjpegDecoder { MppBufferGroup mpp_packet_group_ = nullptr; MppTask mpp_task_ = nullptr; uint32_t need_split_ = 0; - uint8_t * rgb_buffer_ = nullptr; + uint8_t* rgb_buffer_ = nullptr; }; } // namespace orbbec_camera \ No newline at end of file diff --git a/orbbec_camera/include/orbbec_camera/utils.h b/orbbec_camera/include/orbbec_camera/utils.h index a38e5c7c..77c58b10 100644 --- a/orbbec_camera/include/orbbec_camera/utils.h +++ b/orbbec_camera/include/orbbec_camera/utils.h @@ -53,7 +53,7 @@ bool isOpenNIDevice(int pid); OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString( const std::string& depth_precision_level_str); -OBSyncMode OBSyncModeFromString(const std::string& mode); +OBMultiDeviceSyncMode OBSyncModeFromString(const std::string& mode); OB_SAMPLE_RATE sampleRateFromString(std::string& sample_rate); diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index c3bb773a..b5aa7530 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index 183696b7..cf93c8ba 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -67,9 +67,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/astra_adv.launch.py b/orbbec_camera/launch/astra_adv.launch.py index 1621ab3f..62aceac0 100644 --- a/orbbec_camera/launch/astra_adv.launch.py +++ b/orbbec_camera/launch/astra_adv.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/astra_embedded_s.launch.py b/orbbec_camera/launch/astra_embedded_s.launch.py index db8e6720..4e9bfcb8 100644 --- a/orbbec_camera/launch/astra_embedded_s.launch.py +++ b/orbbec_camera/launch/astra_embedded_s.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/astra_stereo_u3.launch.py b/orbbec_camera/launch/astra_stereo_u3.launch.py index ebd9cd52..87695afc 100644 --- a/orbbec_camera/launch/astra_stereo_u3.launch.py +++ b/orbbec_camera/launch/astra_stereo_u3.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/dabai.launch.py b/orbbec_camera/launch/dabai.launch.py index ebd9cd52..87695afc 100644 --- a/orbbec_camera/launch/dabai.launch.py +++ b/orbbec_camera/launch/dabai.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/deeya.launch.py b/orbbec_camera/launch/deeya.launch.py index db8e6720..4e9bfcb8 100644 --- a/orbbec_camera/launch/deeya.launch.py +++ b/orbbec_camera/launch/deeya.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 2efbac4c..0c0f91c6 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 22e6ec86..7a890d43 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index 34e828e2..91c58c23 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -67,9 +67,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 4111ce43..f5a32e3a 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -67,9 +67,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/gemini2XL.launch.py b/orbbec_camera/launch/gemini2XL.launch.py new file mode 100644 index 00000000..97b48cac --- /dev/null +++ b/orbbec_camera/launch/gemini2XL.launch.py @@ -0,0 +1,112 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace +from launch.actions import GroupAction +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # Declare arguments + args = [ + DeclareLaunchArgument('camera_name', default_value='camera'), + DeclareLaunchArgument('depth_registration', default_value='false'), + DeclareLaunchArgument('serial_number', default_value=''), + DeclareLaunchArgument('usb_port', default_value=''), + DeclareLaunchArgument('device_num', default_value='1'), + DeclareLaunchArgument('vendor_id', default_value='0x2bc5'), + DeclareLaunchArgument('product_id', default_value=''), + DeclareLaunchArgument('enable_point_cloud', default_value='true'), + DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'), + DeclareLaunchArgument('point_cloud_qos', default_value='default'), + DeclareLaunchArgument('connection_delay', default_value='100'), + DeclareLaunchArgument('color_width', default_value='640'), + DeclareLaunchArgument('color_height', default_value='400'), + DeclareLaunchArgument('color_fps', default_value='10'), + DeclareLaunchArgument('color_format', default_value='MJPG'), + DeclareLaunchArgument('enable_color', default_value='true'), + DeclareLaunchArgument('flip_color', default_value='false'), + DeclareLaunchArgument('color_qos', default_value='default'), + DeclareLaunchArgument('color_camera_info_qos', default_value='default'), + DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'), + DeclareLaunchArgument('depth_width', default_value='640'), + DeclareLaunchArgument('depth_height', default_value='400'), + DeclareLaunchArgument('depth_fps', default_value='10'), + DeclareLaunchArgument('depth_format', default_value='Y16'), + DeclareLaunchArgument('enable_depth', default_value='true'), + DeclareLaunchArgument('flip_depth', default_value='false'), + DeclareLaunchArgument('depth_qos', default_value='default'), + DeclareLaunchArgument('depth_camera_info_qos', default_value='default'), + DeclareLaunchArgument('left_ir_width', default_value='640'), + DeclareLaunchArgument('left_ir_height', default_value='400'), + DeclareLaunchArgument('left_ir_fps', default_value='10'), + DeclareLaunchArgument('left_ir_format', default_value='Y8'), + DeclareLaunchArgument('enable_left_ir', default_value='true'), + DeclareLaunchArgument('flip_left_ir', default_value='false'), + DeclareLaunchArgument('left_ir_qos', default_value='default'), + DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'), + DeclareLaunchArgument('right_ir_width', default_value='640'), + DeclareLaunchArgument('right_ir_height', default_value='400'), + DeclareLaunchArgument('right_ir_fps', default_value='10'), + DeclareLaunchArgument('right_ir_format', default_value='Y8'), + DeclareLaunchArgument('enable_right_ir', default_value='true'), + DeclareLaunchArgument('flip_right_ir', default_value='false'), + DeclareLaunchArgument('right_ir_qos', default_value='default'), + DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'), + DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'), + DeclareLaunchArgument('enable_accel', default_value='false'), + DeclareLaunchArgument('accel_rate', default_value='100hz'), + DeclareLaunchArgument('accel_range', default_value='4g'), + DeclareLaunchArgument('enable_gyro', default_value='false'), + DeclareLaunchArgument('gyro_rate', default_value='100hz'), + DeclareLaunchArgument('gyro_range', default_value='1000dps'), + DeclareLaunchArgument('liner_accel_cov', default_value='0.01'), + DeclareLaunchArgument('angular_vel_cov', default_value='0.01'), + DeclareLaunchArgument('publish_tf', default_value='true'), + DeclareLaunchArgument('tf_publish_rate', default_value='10.0'), + DeclareLaunchArgument('ir_info_url', default_value=''), + DeclareLaunchArgument('color_info_url', default_value=''), + DeclareLaunchArgument('log_level', default_value='none'), + DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), + DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), + DeclareLaunchArgument('enable_soft_filter', default_value='true'), + DeclareLaunchArgument('enable_ldp', default_value='true'), + DeclareLaunchArgument('enable_soft_filter', default_value='true'), + DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), + DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), + ] + + # Node configuration + parameters = [{arg.name: LaunchConfiguration(arg.name)} for arg in args] + # Define the ComposableNode + compose_node = ComposableNode( + package='orbbec_camera', + plugin='orbbec_camera::OBCameraNodeDriver', + name=LaunchConfiguration('camera_name'), + namespace='', + parameters=parameters, + ) + # Define the ComposableNodeContainer + container = ComposableNodeContainer( + name='camera_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + compose_node, + ], + output='screen', + ) + + # Launch description + ld = LaunchDescription( + args + + [ + GroupAction([ + PushRosNamespace(LaunchConfiguration('camera_name')), + container + ]) + ] + ) + return ld diff --git a/orbbec_camera/launch/gemini_e.launch.py b/orbbec_camera/launch/gemini_e.launch.py index 91a7971a..81ec1669 100644 --- a/orbbec_camera/launch/gemini_e.launch.py +++ b/orbbec_camera/launch/gemini_e.launch.py @@ -59,9 +59,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/launch/ob_camera.launch.py b/orbbec_camera/launch/ob_camera.launch.py index 34e828e2..91c58c23 100644 --- a/orbbec_camera/launch/ob_camera.launch.py +++ b/orbbec_camera/launch/ob_camera.launch.py @@ -67,9 +67,6 @@ def generate_launch_description(): DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'), - DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'), - DeclareLaunchArgument('video_convert', default_value='videoconvert'), - DeclareLaunchArgument('jpeg_parse', default_value='jpegparse'), ] # Node configuration diff --git a/orbbec_camera/src/gst_decoder.cpp b/orbbec_camera/src/gst_decoder.cpp deleted file mode 100644 index eb40e875..00000000 --- a/orbbec_camera/src/gst_decoder.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "orbbec_camera/gst_decoder.h" -#include -#include -#include -#include -#include - -namespace orbbec_camera { - -GstreamerMjpegDecoder::GstreamerMjpegDecoder(int width, int height, std::string jpeg_decoder, - std::string video_convert, std::string jpeg_parse) - : MjpegDecoder(width, height), - jpeg_decoder_(std::move(jpeg_decoder)), - video_convert_(std::move(video_convert)), - jpeg_parse_(std::move(jpeg_parse)), - buffer_size_(width * height * 3) { - gst_init(NULL, NULL); - buffer_pool_ = gst_buffer_pool_new(); - CHECK_NOTNULL(buffer_pool_); - GstStructure* config = gst_buffer_pool_get_config(buffer_pool_); - CHECK_NOTNULL(config); - gst_buffer_pool_config_set_params(config, NULL, buffer_size_, 0, 0); - if (jpeg_decoder_ == "unknown") { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "hw decoder is unknown"); - throw std::runtime_error("hw decoder is unknown"); - } - if (!gst_buffer_pool_set_config(buffer_pool_, config)) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "gst buffer pool set config error"); - throw std::runtime_error("gst buffer pool set config error"); - } - if (gst_buffer_pool_set_active(buffer_pool_, TRUE) != TRUE) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "gst buffer pool set active error"); - throw std::runtime_error("gst buffer pool set active error"); - } - // Create GStreamer elements - RCLCPP_INFO_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "hw decoder: " << jpeg_decoder_ << ", video convert: " << video_convert_ - << ", jpeg parse: " << jpeg_parse_); - appsrc_ = gst_element_factory_make("appsrc", "appsrc"); - jpegparse_ = gst_element_factory_make(jpeg_parse_.c_str(), jpeg_parse_.c_str()); - jpegdec_ = gst_element_factory_make(jpeg_decoder_.c_str(), jpeg_decoder_.c_str()); - videoconvert_ = gst_element_factory_make(video_convert_.c_str(), video_convert_.c_str()); - appsink_ = gst_element_factory_make("appsink", "appsink"); - - // Check for null pointers - if (!appsrc_ || !jpegparse_ || !jpegdec_ || !videoconvert_ || !appsink_) { - throw std::runtime_error("Failed to create GStreamer elements"); - } - - // Set element properties - g_object_set(G_OBJECT(appsrc_), "caps", - gst_caps_new_simple("image/jpeg", "width", G_TYPE_INT, width, "height", G_TYPE_INT, - height, "framerate", GST_TYPE_FRACTION, 0, 1, NULL), - NULL); - - g_object_set(G_OBJECT(appsink_), "blocksize", buffer_size_, "sync", FALSE, "max-buffers", 1, - "drop", TRUE, NULL); - - g_object_set(G_OBJECT(appsink_), "caps", - gst_caps_new_simple("video/x-raw", "format", G_TYPE_STRING, "RGB", "width", - G_TYPE_INT, width, "height", G_TYPE_INT, height, NULL), - NULL); - - // Create pipeline and add elements - pipeline_ = gst_pipeline_new("pipeline"); - if (!pipeline_) { - throw std::runtime_error("Failed to create pipeline"); - } - - gst_bin_add_many(GST_BIN(pipeline_), appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_, - NULL); - if (!gst_element_link_many(appsrc_, jpegparse_, jpegdec_, videoconvert_, appsink_, NULL)) { - throw std::runtime_error("Failed to link GStreamer elements"); - } - // Set pipeline to playing state - if (gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { - throw std::runtime_error("Failed to set GStreamer pipeline to playing state"); - } -} - -GstreamerMjpegDecoder::~GstreamerMjpegDecoder() { - if (pipeline_) { - gst_element_set_state(pipeline_, GST_STATE_NULL); - gst_object_unref(pipeline_); - pipeline_ = nullptr; - } - if (buffer_pool_) { - gst_buffer_pool_set_active(buffer_pool_, FALSE); - g_object_unref(buffer_pool_); - } - gst_deinit(); -} - -bool GstreamerMjpegDecoder::decode(const std::shared_ptr& frame, uint8_t* dest) { - GstBuffer* buffer = NULL; - GstMapInfo map; - // Acquire buffer from pool - if (gst_buffer_pool_acquire_buffer(buffer_pool_, &buffer, NULL) != GST_FLOW_OK) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "gst buffer pool acquire buffer error"); - return false; - } - // Map buffer and copy frame data - if (gst_buffer_map(buffer, &map, GST_MAP_WRITE)) { - if (frame->dataSize() <= map.size) { - memcpy(map.data, frame->data(), frame->dataSize()); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "Frame data size exceeds buffer size"); - gst_buffer_unmap(buffer, &map); - return false; - } - gst_buffer_unmap(buffer, &map); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), "Failed to map buffer"); - return false; - } - - // Push buffer to appsrc - GstFlowReturn flow_return = gst_app_src_push_buffer(GST_APP_SRC(appsrc_), buffer); - if (flow_return != GST_FLOW_OK) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "Failed to push buffer to appsrc, GstFlowReturn: " << flow_return); - return false; - } - - // Pull sample from appsink - GstClockTime timeout = GST_SECOND; - GstSample* sample = gst_app_sink_try_pull_sample(GST_APP_SINK(appsink_), timeout); - - if (sample) { - GstBuffer* outBuffer = gst_sample_get_buffer(sample); - GstMapInfo mapInfo; - if (gst_buffer_map(outBuffer, &mapInfo, GST_MAP_READ)) { - // Copy data to destination buffer - memcpy(dest, mapInfo.data, mapInfo.size); - gst_buffer_unmap(outBuffer, &mapInfo); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "Failed to map output buffer"); - gst_sample_unref(sample); - return false; - } - gst_sample_unref(sample); - return true; - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gstreamer_mjpeg_decoder"), - "Failed to decode frame " << strerror(errno) << " " << errno); - return false; - } -} - -} // namespace orbbec_camera \ No newline at end of file diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 22c4f728..bb63cd28 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -20,8 +20,6 @@ #if defined(USE_RK_HW_DECODER) #include "orbbec_camera/rk_mpp_decoder.h" -#elif defined(USE_GST_HW_DECODER) -#include "orbbec_camera/gst_decoder.h" #endif namespace orbbec_camera { @@ -37,7 +35,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic stream_name_[COLOR] = "color"; stream_name_[DEPTH] = "depth"; stream_name_[INFRA0] = "ir"; - stream_name_[INFRA1] = "ir2"; + stream_name_[INFRA1] = "left_ir"; + stream_name_[INFRA2] = "right_ir"; stream_name_[ACCEL] = "accel"; stream_name_[GYRO] = "gyro"; @@ -49,9 +48,6 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr devic setupTopics(); #if defined(USE_RK_HW_DECODER) mjpeg_decoder_ = std::make_unique(width_[COLOR], height_[COLOR]); -#elif defined(USE_GST_HW_DECODER) - mjpeg_decoder_ = std::make_unique( - width_[COLOR], height_[COLOR], jpeg_decoder_, video_convert_, jpeg_parse_); #endif startStreams(); if (enable_d2c_viewer_) { @@ -124,17 +120,15 @@ void OBCameraNode::setupDevices() { if (!depth_work_mode_.empty()) { device_->switchDepthWorkMode(depth_work_mode_.c_str()); } - if (sync_mode_ != OB_SYNC_MODE_CLOSE) { - OBDeviceSyncConfig sync_config; + if (sync_mode_ != OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN) { + auto sync_config = device_->getMultiDeviceSyncConfig(); sync_config.syncMode = sync_mode_; - sync_config.irTriggerSignalInDelay = ir_trigger_signal_in_delay_; - sync_config.rgbTriggerSignalInDelay = rgb_trigger_signal_in_delay_; - sync_config.deviceTriggerSignalOutDelay = device_trigger_signal_out_delay_; - device_->setSyncConfig(sync_config); - if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL, - OB_PERMISSION_READ_WRITE)) { - device_->setBoolProperty(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL, sync_signal_trigger_out_); - } + sync_config.depthDelayUs = depth_delay_us_; + sync_config.colorDelayUs = color_delay_us_; + sync_config.trigger2ImageDelayUs = trigger2image_delay_us_; + sync_config.triggerSignalOutputDelayUs = trigger_signal_output_delay_us_; + sync_config.triggerSignalOutputEnable = trigger_signal_output_enabled_; + device_->setMultiDeviceSyncConfig(sync_config); } if (info->pid() == GEMINI2_PID) { auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT); @@ -194,7 +188,7 @@ void OBCameraNode::setupProfiles() { << ", Stream Index: " << elem.second << ", Width: " << width_[elem] << ", Height: " << height_[elem] << ", FPS: " << fps_[elem] << ", Format: " << magic_enum::enum_name(format_[elem])); - throw; + exit(-1); } if (!selected_profile) { @@ -334,6 +328,18 @@ void OBCameraNode::setupDefaultImageFormat() { encoding_[INFRA0] = sensor_msgs::image_encodings::MONO16; unit_step_size_[INFRA0] = sizeof(uint16_t); + format_[INFRA1] = OB_FORMAT_Y16; + format_str_[INFRA1] = "Y16"; + image_format_[INFRA1] = CV_16UC1; + encoding_[INFRA1] = sensor_msgs::image_encodings::MONO16; + unit_step_size_[INFRA1] = sizeof(uint16_t); + + format_[INFRA2] = OB_FORMAT_Y16; + format_str_[INFRA2] = "Y16"; + image_format_[INFRA2] = CV_16UC1; + encoding_[INFRA2] = sensor_msgs::image_encodings::MONO16; + unit_step_size_[INFRA2] = sizeof(uint16_t); + image_format_[COLOR] = CV_8UC3; encoding_[COLOR] = sensor_msgs::image_encodings::RGB8; unit_step_size_[COLOR] = 3 * sizeof(uint8_t); @@ -404,7 +410,6 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(enable_colored_point_cloud_, "enable_colored_point_cloud", false); setAndGetNodeParameter(enable_point_cloud_, "enable_point_cloud", true); setAndGetNodeParameter(point_cloud_qos_, "point_cloud_qos", "default"); - setAndGetNodeParameter(enable_publish_extrinsic_, "enable_publish_extrinsic", false); setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false); setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true); setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true); @@ -412,10 +417,11 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true); setAndGetNodeParameter(depth_work_mode_, "depth_work_mode", ""); setAndGetNodeParameter(sync_mode_str_, "sync_mode", "close"); - setAndGetNodeParameter(ir_trigger_signal_in_delay_, "ir_trigger_signal_in_delay", 0); - setAndGetNodeParameter(rgb_trigger_signal_in_delay_, "rgb_trigger_signal_in_delay", 0); - setAndGetNodeParameter(device_trigger_signal_out_delay_, "device_trigger_signal_out_delay", 0); - setAndGetNodeParameter(sync_signal_trigger_out_, "sync_signal_trigger_out", false); + setAndGetNodeParameter(depth_delay_us_, "depth_delay_us", 0); + setAndGetNodeParameter(color_delay_us_, "color_delay_us", 0); + setAndGetNodeParameter(trigger2image_delay_us_, "trigger2image_delay_us", 0); + setAndGetNodeParameter(trigger_signal_output_delay_us_, "trigger_signal_output_delay_us", 0); + setAndGetNodeParameter(trigger_signal_output_enabled_, "trigger_signal_output_enabled", false); setAndGetNodeParameter(depth_precision_str_, "depth_precision", "1mm"); std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper); sync_mode_ = OBSyncModeFromString(sync_mode_str_); @@ -428,9 +434,6 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(soft_filter_speckle_size_, "soft_filter_speckle_size", -1); setAndGetNodeParameter(liner_accel_cov_, "linear_accel_cov", 0.0003); setAndGetNodeParameter(angular_vel_cov_, "angular_vel_cov", 0.02); - setAndGetNodeParameter(jpeg_decoder_, "jpeg_decoder", "avdec_mjpeg"); - setAndGetNodeParameter(video_convert_, "video_convert", "videoconvert"); - setAndGetNodeParameter(jpeg_parse_, "jpeg_parse", "jpegparse"); } void OBCameraNode::setupTopics() { @@ -502,10 +505,6 @@ void OBCameraNode::setupPublishers() { imu_publishers_[stream_index] = node_->create_publisher( data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); } - if (enable_publish_extrinsic_) { - extrinsics_publisher_ = node_->create_publisher( - "extrinsic/depth_to_color", rclcpp::QoS{1}.transient_local()); - } } void OBCameraNode::publishPointCloud(const std::shared_ptr &frame_set) { @@ -702,12 +701,16 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr &fr tf_published_ = true; } publishPointCloud(frame_set); - auto color_frame = std::dynamic_pointer_cast(frame_set->colorFrame()); - auto depth_frame = std::dynamic_pointer_cast(frame_set->depthFrame()); - auto ir_frame = std::dynamic_pointer_cast(frame_set->irFrame()); - onNewFrameCallback(color_frame, COLOR); - onNewFrameCallback(depth_frame, DEPTH); - onNewFrameCallback(ir_frame, INFRA0); + for (const auto &stream_index : IMAGE_STREAMS) { + if (enable_stream_[stream_index]) { + auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first); + auto frame = frame_set->getFrame(frame_type); + if (frame == nullptr) { + continue; + } + onNewFrameCallback(frame, stream_index); + } + } } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage()); } catch (const std::exception &e) { @@ -748,7 +751,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, auto frame_format = frame->format(); if (frame->type() == OB_FRAME_COLOR && frame_format != OB_FORMAT_RGB888) { if (frame_format == OB_FORMAT_MJPG || frame_format == OB_FORMAT_MJPEG) { -#if defined(USE_RK_HW_DECODER) || defined(USE_GST_HW_DECODER) +#if defined(USE_RK_HW_DECODER) CHECK_NOTNULL(mjpeg_decoder_.get()); video_frame = frame->as(); const auto &color_frame = frame->as(); @@ -773,7 +776,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, video_frame = frame->as(); } else if (frame->type() == OB_FRAME_DEPTH) { video_frame = frame->as(); - } else if (frame->type() == OB_FRAME_IR) { + } else if (frame->type() == OB_FRAME_IR || frame->type() == OB_FRAME_IR_LEFT || + frame->type() == OB_FRAME_IR_RIGHT) { video_frame = frame->as(); } else { RCLCPP_ERROR(logger_, "Unsupported frame type: %d", frame->type()); diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 27ba9747..03f9053e 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include namespace orbbec_camera { OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options) @@ -86,7 +86,6 @@ void OBCameraNodeDriver::init() { RCLCPP_INFO_STREAM(rclcpp::get_logger("orbbec_camera_node_driver"), "SIGTERM received"); exit(0); }); - } void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr &device_list) { @@ -185,7 +184,7 @@ void OBCameraNodeDriver::deviceCountUpdate() { void OBCameraNodeDriver::syncTime() { while (is_alive_ && rclcpp::ok()) { if (device_ && device_info_ && !isOpenNIDevice(device_info_->pid())) { - ctx_->enableMultiDeviceSync(0); + ctx_->enableDeviceClockSync(0); } std::this_thread::sleep_for(std::chrono::milliseconds(5000)); } @@ -386,7 +385,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &dev CHECK_NOTNULL(device_info_.get()); device_unique_id_ = device_info_->uid(); if (!isOpenNIDevice(device_info_->pid())) { - ctx_->enableMultiDeviceSync(0); // sync time stamp + ctx_->enableDeviceClockSync(0); // sync time stamp } RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->name() << " connected"); RCLCPP_INFO_STREAM(logger_, "Serial number: " << device_info_->serialNumber()); diff --git a/orbbec_camera/src/rk_mpp_decoder.cpp b/orbbec_camera/src/rk_mpp_decoder.cpp index 32c56773..59500493 100644 --- a/orbbec_camera/src/rk_mpp_decoder.cpp +++ b/orbbec_camera/src/rk_mpp_decoder.cpp @@ -101,17 +101,12 @@ RKMjpegDecoder::~RKMjpegDecoder() { mpp_destroy(mpp_ctx_); mpp_ctx_ = nullptr; } - if(rgb_buffer_){ + if (rgb_buffer_) { delete[] rgb_buffer_; } } bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) { - rga_info_t src_info; - rga_info_t dst_info; - // NOTE: memset to zero is MUST - memset(&src_info, 0, sizeof(rga_info_t)); - memset(&dst_info, 0, sizeof(rga_info_t)); int width = mpp_frame_get_width(frame); int height = mpp_frame_get_height(frame); MppBuffer buffer = mpp_frame_get_buffer(frame); @@ -122,6 +117,19 @@ bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) { CHECK_EQ(height, height_); memset(data, 0, width * height * 3); auto buffer_ptr = mpp_buffer_get_ptr(buffer); +#if defined(USE_LIBYUV) + // use libyuv to convert yuv420sp to rgb888 + libyuv::I420ToRGB24((const uint8_t *)buffer_ptr, width, + (const uint8_t *)buffer_ptr + width * height, width / 2, + (const uint8_t *)buffer_ptr + width * height * 5 / 4, width / 2, data, + width * 3, width, height); + return true; +#else + rga_info_t src_info; + rga_info_t dst_info; + // NOTE: memset to zero is MUST + memset(&src_info, 0, sizeof(rga_info_t)); + memset(&dst_info, 0, sizeof(rga_info_t)); src_info.fd = -1; src_info.mmuFlag = 1; src_info.virAddr = buffer_ptr; @@ -139,6 +147,7 @@ bool RKMjpegDecoder::mppFrame2RGB(const MppFrame frame, uint8_t *data) { return false; } return true; +#endif } bool RKMjpegDecoder::decode(const std::shared_ptr &frame, uint8_t *dest) { diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp index cd9e7b12..ec19e4f2 100644 --- a/orbbec_camera/src/utils.cpp +++ b/orbbec_camera/src/utils.cpp @@ -281,25 +281,23 @@ OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString( return OB_PRECISION_0MM8; } } - -OBSyncMode OBSyncModeFromString(const std::string &mode) { - if (mode == "CLOSE") { - return OBSyncMode::OB_SYNC_MODE_CLOSE; +OBMultiDeviceSyncMode OBSyncModeFromString(const std::string &mode) { + if (mode == "FREE_RUN") { + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN; } else if (mode == "STANDALONE") { - return OBSyncMode::OB_SYNC_MODE_STANDALONE; + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_STANDALONE; + } else if (mode == "PRIMARY") { + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_PRIMARY; } else if (mode == "SECONDARY") { - return OBSyncMode::OB_SYNC_MODE_SECONDARY; - } else if (mode == "PRIMARY_MCU_TRIGGER") { - return OBSyncMode::OB_SYNC_MODE_PRIMARY_MCU_TRIGGER; - } else if (mode == "PRIMARY_IR_TRIGGER") { - return OBSyncMode::OB_SYNC_MODE_PRIMARY_IR_TRIGGER; - } else if (mode == "PRIMARY_SOFT_TRIGGER") { - return OBSyncMode::OB_SYNC_MODE_PRIMARY_SOFT_TRIGGER; - } else if (mode == "SECONDARY_SOFT_TRIGGER") { - return OBSyncMode::OB_SYNC_MODE_SECONDARY_SOFT_TRIGGER; + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY; + } else if (mode == "SECONDARY_SYNCED") { + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY_SYNCED; + } else if (mode == "SOFTWARE_TRIGGERING") { + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING; + } else if (mode == "HARDWARE_TRIGGERING") { + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING; } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("utils"), "Unknown OBSyncMode: " << mode); - return OBSyncMode::OB_SYNC_MODE_CLOSE; + return OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN; } }