Add obsdk v2.0.13 and update rossdk v2.0.4

.vscode/settings.json

@@ -0,0 +1,76 @@
+{
+    "files.associations": {
+        "*.tcc": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "string": "cpp",
+        "vector": "cpp",
+        "valarray": "cpp",
+        "array": "cpp",
+        "string_view": "cpp",
+        "chrono": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "bitset": "cpp",
+        "cinttypes": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "semaphore": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp"
+    }
+}

orbbec_camera/SDK/include/libobsensor/ObSensor.h

@@ -1,5 +1,5 @@
-/* License: Apache 2.0. See LICENSE file in root directory.
+// Copyright (c) Orbbec Inc. All Rights Reserved.
-    Copyright(c) 2020  Orbbec Corporation. All Rights Reserved. */
+// Licensed under the MIT License.
 
 /**
  * \file ObSensor.h
@@ -20,3 +20,4 @@
 #include <libobsensor/h/StreamProfile.h>
 #include <libobsensor/h/Version.h>
 #include <libobsensor/h/TypeHelper.h>
+

orbbec_camera/SDK/include/libobsensor/ObSensor.hpp

@@ -1,5 +1,5 @@
-/* License: Apache 2.0. See LICENSE file in root directory.
+// Copyright (c) Orbbec Inc. All Rights Reserved.
-    Copyright(c) 2020  Orbbec Corporation. All Rights Reserved. */
+// Licensed under the MIT License.
 
 /**
  * \file ObSensor.hpp
@@ -19,3 +19,4 @@
 #include <libobsensor/hpp/StreamProfile.hpp>
 #include <libobsensor/hpp/Version.hpp>
 #include <libobsensor/hpp/TypeHelper.hpp>
+

orbbec_camera/SDK/include/libobsensor/h/Advanced.h

@@ -1,3 +1,6 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 #ifdef __cplusplus
 extern "C" {
 #endif

orbbec_camera/SDK/include/libobsensor/h/Context.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Context.h
  * @brief Context is a management class that describes the runtime of the SDK and is responsible for resource allocation and release of the SDK.
  * Context has the ability to manage multiple devices. It is responsible for enumerating devices, monitoring device callbacks, and enabling multi-device

orbbec_camera/SDK/include/libobsensor/h/Device.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Device.h
  * @brief Device-related functions, including operations such as obtaining and creating a device, setting and obtaining device property, and obtaining sensors
  */
@@ -641,3 +644,4 @@ OB_EXPORT void ob_delete_camera_param_list(ob_camera_param_list *param_list, ob_
 #ifdef __cplusplus
 }
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/Error.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Error.h
  * @brief Functions for handling errors, mainly used for obtaining error messages.
  */

orbbec_camera/SDK/include/libobsensor/h/Filter.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Filter.h
  * @brief The processing unit of the SDK can perform point cloud generation, format conversion and other functions.
  */
@@ -249,3 +252,4 @@ OB_EXPORT ob_filter_config_schema_item ob_filter_config_schema_list_get_item(con
 #ifdef __cplusplus
 }
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/Frame.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Frame.h
  * @brief Frame related function is mainly used to obtain frame data and frame information
  *
@@ -634,3 +637,4 @@ OB_EXPORT void ob_frameset_push_frame(ob_frame *frameset, const ob_frame *frame,
 #ifdef __cplusplus
 }
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/MultipleDevices.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file MultipleDevices.h
  * @brief This file contains the multiple devices related API witch is used to control the synchronization between multiple devices and the synchronization
  * between different sensor within single device.
@@ -122,3 +125,4 @@ OB_EXPORT void ob_device_timer_sync_with_host(ob_device *device, ob_error **erro
 #ifdef __cplusplus
 }  // extern "C"
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/ObTypes.h

@@ -1,4 +1,7 @@
-// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+// License: Apache 2.0. See LICENSE file in root directory.
 // Copyright(c) 2020 Orbbec  Corporation. All Rights Reserved.
 
 /**
@@ -968,7 +971,7 @@ typedef struct {
 } OBEdgeNoiseRemovalFilterParams, ob_edge_noise_removal_filter_params;
 
 /**
- * @brief 去噪方式
+ * @brief Denoising method
  */
 typedef enum OB_DDO_NOISE_REMOVAL_TYPE {
     OB_NR_LUT     = 0,  // SPLIT
@@ -1750,3 +1753,4 @@ typedef void(ob_log_callback)(ob_log_severity severity, const char *message, voi
 #endif
 
 #pragma pack(pop)
+

orbbec_camera/SDK/include/libobsensor/h/Pipeline.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Pipeline.h
  * @brief The SDK's advanced API can quickly implement functions such as switching streaming, frame synchronization, software filtering, etc., suitable for
  * applications, and the algorithm focuses on rgbd data stream scenarios. If you are on real-time or need to handle synchronization separately, align the scene.
@@ -329,3 +332,4 @@ OB_EXPORT ob_calibration_param ob_pipeline_get_calibration_param(ob_pipeline *pi
 #ifdef __cplusplus
 }
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/Property.h

@@ -1,5 +1,5 @@
-// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright (c) Orbbec Inc. All Rights Reserved.
-// Copyright(c) 2020 Orbbec  Corporation. All Rights Reserved.
+// Licensed under the MIT License.
 
 /**
  * @file Property.h
@@ -771,3 +771,4 @@ typedef struct OBPropertyItem {
 #ifdef __cplusplus
 }
 #endif
+

orbbec_camera/SDK/include/libobsensor/h/Sensor.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Sensor.h
  * @brief Defines types related to sensors, used for obtaining stream configurations, opening and closing streams, and setting and getting sensor properties.
  */

orbbec_camera/SDK/include/libobsensor/h/StreamProfile.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file StreamProfile.h
  * @brief The stream profile related type is used to get information such as the width, height, frame rate, and format of the stream.
  *

orbbec_camera/SDK/include/libobsensor/h/TypeHelper.h

@@ -1,3 +1,6 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 #pragma once
 
 #ifdef __cplusplus

orbbec_camera/SDK/include/libobsensor/h/Utils.h

@@ -1,3 +1,6 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 #pragma once
 
 #ifdef __cplusplus

orbbec_camera/SDK/include/libobsensor/h/Version.h

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Version.h
  * @brief Functions for retrieving the SDK version number information.
  *

orbbec_camera/SDK/include/libobsensor/hpp/Context.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Context.hpp
  * @brief The SDK context class, which serves as the entry point to the underlying SDK. It is used to query device lists, handle device callbacks, and set the
  * log level.
@@ -224,3 +227,4 @@ private:
 #define enableMultiDeviceSync enableDeviceClockSync
 };
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Device.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Device.hpp
  * @brief Device related types, including operations such as getting and creating a device, setting and obtaining device attributes, and obtaining sensors
  *
@@ -1358,3 +1361,4 @@ public:
 };
 
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Error.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file  Error.hpp
  * @brief This file defines the Error class, which describes abnormal errors within the SDK.
  *        Detailed information about the exception can be obtained through this class.
@@ -108,3 +111,4 @@ public:
     }
 };
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Filter.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file  Filter.hpp
  * @brief This file contains the Filter class, which is the processing unit of the SDK that can perform point cloud generation, format conversion, and other
  * functions.
@@ -1007,3 +1010,4 @@ template <typename T> bool Filter::is() {
 }
 
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Frame.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Frame.hpp
  * @brief Frame related type, which is mainly used to obtain frame data and frame information.
  *

orbbec_camera/SDK/include/libobsensor/hpp/Pipeline.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Pipeline.hpp
  * @brief The SDK's advanced API type can quickly implement switching streaming and frame synchronization
  * operations.
@@ -446,3 +449,4 @@ public:
 };
 
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Sensor.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Sensor.hpp
  * @brief Defines types related to sensors, which are used to obtain stream configurations, open and close streams, and set and get sensor properties.
  */
@@ -230,3 +233,4 @@ public:
 };
 
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/StreamProfile.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file StreamProfile.hpp
  * @brief The stream profile related type is used to get information such as the width, height, frame rate, and format of the stream.
  */
@@ -439,3 +442,4 @@ public:
 };
 
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/TypeHelper.hpp

@@ -1,3 +1,6 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 #pragma once
 
 #include <string>
@@ -137,3 +140,4 @@ public:
     }
 };
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Types.hpp

@@ -1,2 +1,5 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 #pragma once
 // empty file, leave here for backwards compatibility

orbbec_camera/SDK/include/libobsensor/hpp/Utils.hpp

@@ -1,3 +1,6 @@
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
 /**
  * @file Utils.hpp
  * @brief The SDK utils class
@@ -161,3 +164,4 @@ public:
     }
 };
 }  // namespace ob
+

orbbec_camera/SDK/include/libobsensor/hpp/Version.hpp

@@ -1,4 +1,7 @@
-/**
+// Copyright (c) Orbbec Inc. All Rights Reserved.
+// Licensed under the MIT License.
+
+/**
  * @file Version.hpp
  * @brief Provides functions to retrieve version information of the SDK.
  */
@@ -56,3 +59,4 @@ public:
     }
 };
 }  // namespace ob
+

orbbec_camera/SDK/lib/x64/OrbbecSDKConfig-release.cmake

@@ -12,8 +12,8 @@ set_target_properties(ob::OrbbecSDK PROPERTIES
   IMPORTED_SONAME_RELEASE "libOrbbecSDK.so"
   )
 
-list(APPEND _IMPORT_CHECK_TARGETS ob::OrbbecSDK )
+list(APPEND _cmake_import_check_targets ob::OrbbecSDK )
-list(APPEND _IMPORT_CHECK_FILES_FOR_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so" )
+list(APPEND _cmake_import_check_files_for_ob::OrbbecSDK "${_IMPORT_PREFIX}/lib/libOrbbecSDK.so" )
 
 # Commands beyond this point should not need to know the version.
 set(CMAKE_IMPORT_FILE_VERSION)

orbbec_camera/SDK/lib/x64/OrbbecSDKConfig.cmake

@@ -1,10 +1,13 @@
 # Generated by CMake
 
-if("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 2.5)
+if("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 2.8)
-   message(FATAL_ERROR "CMake >= 2.6.0 required")
+   message(FATAL_ERROR "CMake >= 2.8.0 required")
+endif()
+if(CMAKE_VERSION VERSION_LESS "2.8.3")
+   message(FATAL_ERROR "CMake >= 2.8.3 required")
 endif()
 cmake_policy(PUSH)
-cmake_policy(VERSION 2.6)
+cmake_policy(VERSION 2.8.3...3.28)
 #----------------------------------------------------------------
 # Generated CMake target import file.
 #----------------------------------------------------------------
@@ -13,32 +16,34 @@ cmake_policy(VERSION 2.6)
 set(CMAKE_IMPORT_FILE_VERSION 1)
 
 # Protect against multiple inclusion, which would fail when already imported targets are added once more.
-set(_targetsDefined)
+set(_cmake_targets_defined "")
-set(_targetsNotDefined)
+set(_cmake_targets_not_defined "")
-set(_expectedTargets)
+set(_cmake_expected_targets "")
-foreach(_expectedTarget ob::OrbbecSDK)
+foreach(_cmake_expected_target IN ITEMS ob::OrbbecSDK)
-  list(APPEND _expectedTargets ${_expectedTarget})
+  list(APPEND _cmake_expected_targets "${_cmake_expected_target}")
-  if(NOT TARGET ${_expectedTarget})
+  if(TARGET "${_cmake_expected_target}")
-    list(APPEND _targetsNotDefined ${_expectedTarget})
+    list(APPEND _cmake_targets_defined "${_cmake_expected_target}")
-  endif()
+  else()
-  if(TARGET ${_expectedTarget})
+    list(APPEND _cmake_targets_not_defined "${_cmake_expected_target}")
-    list(APPEND _targetsDefined ${_expectedTarget})
   endif()
 endforeach()
-if("${_targetsDefined}" STREQUAL "${_expectedTargets}")
+unset(_cmake_expected_target)
-  unset(_targetsDefined)
+if(_cmake_targets_defined STREQUAL _cmake_expected_targets)
-  unset(_targetsNotDefined)
+  unset(_cmake_targets_defined)
-  unset(_expectedTargets)
+  unset(_cmake_targets_not_defined)
-  set(CMAKE_IMPORT_FILE_VERSION)
+  unset(_cmake_expected_targets)
+  unset(CMAKE_IMPORT_FILE_VERSION)
   cmake_policy(POP)
   return()
 endif()
-if(NOT "${_targetsDefined}" STREQUAL "")
+if(NOT _cmake_targets_defined STREQUAL "")
-  message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_targetsDefined}\nTargets not yet defined: ${_targetsNotDefined}\n")
+  string(REPLACE ";" ", " _cmake_targets_defined_text "${_cmake_targets_defined}")
+  string(REPLACE ";" ", " _cmake_targets_not_defined_text "${_cmake_targets_not_defined}")
+  message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_cmake_targets_defined_text}\nTargets not yet defined: ${_cmake_targets_not_defined_text}\n")
 endif()
-unset(_targetsDefined)
+unset(_cmake_targets_defined)
-unset(_targetsNotDefined)
+unset(_cmake_targets_not_defined)
-unset(_expectedTargets)
+unset(_cmake_expected_targets)
 
 
 # Compute the installation prefix relative to this file.
@@ -56,21 +61,25 @@ set_target_properties(ob::OrbbecSDK PROPERTIES
 )
 
 # Load information for each installed configuration.
-get_filename_component(_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+file(GLOB _cmake_config_files "${CMAKE_CURRENT_LIST_DIR}/OrbbecSDKConfig-*.cmake")
-file(GLOB CONFIG_FILES "${_DIR}/OrbbecSDKConfig-*.cmake")
+foreach(_cmake_config_file IN LISTS _cmake_config_files)
-foreach(f ${CONFIG_FILES})
+  include("${_cmake_config_file}")
-  include(${f})
 endforeach()
+unset(_cmake_config_file)
+unset(_cmake_config_files)
 
 # Cleanup temporary variables.
 set(_IMPORT_PREFIX)
 
 # Loop over all imported files and verify that they actually exist
-foreach(target ${_IMPORT_CHECK_TARGETS} )
+foreach(_cmake_target IN LISTS _cmake_import_check_targets)
-  foreach(file ${_IMPORT_CHECK_FILES_FOR_${target}} )
+  if(CMAKE_VERSION VERSION_LESS "3.28"
-    if(NOT EXISTS "${file}" )
+      OR NOT DEFINED _cmake_import_check_xcframework_for_${_cmake_target}
-      message(FATAL_ERROR "The imported target \"${target}\" references the file
+      OR NOT IS_DIRECTORY "${_cmake_import_check_xcframework_for_${_cmake_target}}")
-   \"${file}\"
+    foreach(_cmake_file IN LISTS "_cmake_import_check_files_for_${_cmake_target}")
+      if(NOT EXISTS "${_cmake_file}")
+        message(FATAL_ERROR "The imported target \"${_cmake_target}\" references the file
+   \"${_cmake_file}\"
 but this file does not exist.  Possible reasons include:
 * The file was deleted, renamed, or moved to another location.
 * An install or uninstall procedure did not complete successfully.
@@ -80,9 +89,12 @@ but not all the files it references.
 ")
       endif()
     endforeach()
-  unset(_IMPORT_CHECK_FILES_FOR_${target})
+  endif()
+  unset(_cmake_file)
+  unset("_cmake_import_check_files_for_${_cmake_target}")
 endforeach()
-unset(_IMPORT_CHECK_TARGETS)
+unset(_cmake_target)
+unset(_cmake_import_check_targets)
 
 # This file does not depend on other imported targets which have
 # been exported from the same project but in a separate export set.

orbbec_camera/include/orbbec_camera/constants.h

@@ -23,7 +23,7 @@
 
 #define OB_ROS_MAJOR_VERSION 2
 #define OB_ROS_MINOR_VERSION 0
-#define OB_ROS_PATCH_VERSION 3
+#define OB_ROS_PATCH_VERSION 4
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -554,6 +554,7 @@ class OBCameraNode {
   std::atomic_bool is_camera_node_initialized_{false};
   int laser_energy_level_ = -1;
   ob::PointCloudFilter depth_point_cloud_filter_;
+  ob::PointCloudFilter color_point_cloud_filter_;
   std::optional<OBCalibrationParam> calibration_param_;
   std::optional<OBXYTables> xy_tables_;
   float* xy_table_data_ = nullptr;

orbbec_camera/launch/gemini_330_series.launch.py

@@ -58,7 +58,7 @@ def generate_launch_description():
         DeclareLaunchArgument('device_num', default_value='1'),
         DeclareLaunchArgument('point_cloud_qos', default_value='default'),
         DeclareLaunchArgument('enable_point_cloud', default_value='true'),
-        DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
+        DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
         DeclareLaunchArgument('cloud_frame_id', default_value=''),
         DeclareLaunchArgument('connection_delay', default_value='10'),
         DeclareLaunchArgument('color_width', default_value='0'),

orbbec_camera/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera</name>
-  <version>2.0.3</version>
+  <version>2.0.4</version>
   <description>Orbbec Camera package</description>
   <maintainer email="[email protected]">Joe Dong</maintainer>
   <license>Apache-2.0</license>

orbbec_camera/src/ob_camera_node.cpp

@@ -552,13 +552,13 @@ void OBCameraNode::setupDepthPostProcessFilter() {
     } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
       auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
       OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
-      RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
+      RCLCPP_INFO_STREAM(
-                                      << "disp_diff: " << params.disp_diff
+          logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
                                                            << ", max_size: " << params.max_size);
       params.disp_diff = noise_removal_filter_min_diff_;
       params.max_size = noise_removal_filter_max_size_;
-      RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
+      RCLCPP_INFO_STREAM(logger_,
-                                      << "disp_diff: " << params.disp_diff
+                         "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
                                                              << ", max_size: " << params.max_size);
       if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
         noise_removal_filter->setFilterParams(params);
@@ -568,8 +568,8 @@ void OBCameraNode::setupDepthPostProcessFilter() {
           hdr_merge_gain_2_ != -1) {
         auto hdr_merge_filter = filter->as<ob::HdrMerge>();
         hdr_merge_filter->enable(true);
-        RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
+        RCLCPP_INFO_STREAM(
-                                        << "exposure_1: " << hdr_merge_exposure_1_
+            logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
                                                      << ", gain_1: " << hdr_merge_gain_1_
                                                      << ", exposure_2: " << hdr_merge_exposure_2_
                                                      << ", gain_2: " << hdr_merge_gain_2_);
@@ -1485,7 +1485,6 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
     }
   }
   depth_cloud_pub_->publish(std::move(point_cloud_msg));
-
 }
 
 void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -1517,39 +1516,28 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
     return;
   }
 
-  if (!xy_tables_.has_value()) {
+  CHECK_NOTNULL(pipeline_);
-    calibration_param_ = pipeline_->getCalibrationParam(pipeline_config_);
+  auto camera_params = pipeline_->getCameraParam();
-
+  auto device_info = device_->getDeviceInfo();
-    uint32_t table_size =
+  CHECK_NOTNULL(device_info.get());
-        color_width * color_height * 2;  // one for x-coordinate and one for y-coordinate LUT
+  auto pid = device_info->pid();
-    if (xy_table_data_size_ != table_size) {
+  if (depth_registration_ || pid == DABAI_MAX_PID) {
-      RCLCPP_INFO_STREAM(logger_, "Init xy tables with size " << table_size);
+    camera_params.depthIntrinsic = camera_params.rgbIntrinsic;
-      xy_table_data_size_ = table_size;
-      delete[] xy_table_data_;
-      xy_table_data_ = new float[table_size];
   }
 
-    xy_tables_ = OBXYTables();
+  color_point_cloud_filter_.setCameraParam(camera_params);
-    CHECK_NOTNULL(xy_table_data_);
+  double depth_scale = depth_frame->getValueScale();
-    if (!ob::CoordinateTransformHelper::transformationInitXYTables(
+  color_point_cloud_filter_.setPositionDataScaled(depth_scale);
-            *calibration_param_, OB_SENSOR_COLOR, xy_table_data_, &table_size, &(*xy_tables_))) {
+  auto alignedFrameset = align_filter_->process(frame_set);
-      RCLCPP_ERROR_STREAM(logger_, "Failed to init xy tables");
+  color_point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
+  auto result_frame = color_point_cloud_filter_.process(alignedFrameset);
+  if (!result_frame) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to process depth frame");
     return;
   }
-  }
+  auto point_size = result_frame->dataSize() / sizeof(OBColorPoint);
-  const auto *depth_data = (uint8_t *)depth_frame->getData();
+  auto *point_cloud = static_cast<OBColorPoint *>(result_frame->data());
-  const auto *color_data = (uint8_t *)(rgb_buffer_);
+
-  CHECK_NOTNULL(rgb_point_cloud_buffer_);
-  uint32_t point_cloud_buffer_size = color_width * color_height * sizeof(OBColorPoint);
-  if (point_cloud_buffer_size > rgb_point_cloud_buffer_size_) {
-    delete[] rgb_point_cloud_buffer_;
-    rgb_point_cloud_buffer_ = new uint8_t[point_cloud_buffer_size];
-    rgb_point_cloud_buffer_size_ = point_cloud_buffer_size;
-  }
-  memset(rgb_point_cloud_buffer_, 0, rgb_point_cloud_buffer_size_);
-  auto *point_cloud = (OBColorPoint *)rgb_point_cloud_buffer_;
-  ob::CoordinateTransformHelper::transformationDepthToRGBDPointCloud(&(*xy_tables_), depth_data,
-                                                                     color_data, point_cloud);
   auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
   sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
   modifier.setPointCloud2FieldsByString(1, "xyz");
@@ -1570,10 +1558,9 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
   size_t valid_count = 0;
   static const float MIN_DISTANCE = 20.0;
   static const float MAX_DISTANCE = 10000.0;
-  double depth_scale = depth_frame->getValueScale();
   static float min_depth = MIN_DISTANCE / depth_scale;
   static float max_depth = MAX_DISTANCE / depth_scale;
-  for (size_t i = 0; i < color_width * color_height; i++) {
+  for (size_t i = 0; i < point_size; i++) {
     bool valid_point = point_cloud[i].z >= min_depth && point_cloud[i].z <= max_depth;
     if (valid_point || ordered_pc_) {
       *iter_x = static_cast<float>(point_cloud[i].x / 1000.0);
@@ -1586,6 +1573,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
       ++valid_count;
     }
   }
+
   if (valid_count == 0) {
     RCLCPP_WARN(logger_, "No valid points in point cloud");
     return;
@@ -1623,6 +1611,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
       RCLCPP_ERROR(logger_, "Failed to save point cloud");
     }
   }
+
   depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
 }