merge to v2-main

.gitignore

@@ -764,6 +764,7 @@ FodyWeavers.xsd
 # JetBrains Rider
 *.sln.iml
 
+
 ### VisualStudio Patch ###
 # Additional files built by Visual Studio
 

.vscode/settings.json

@@ -1,79 +0,0 @@
-{
-    "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",
-        "*.ipp": "cpp",
-        "filesystem": "cpp",
-        "shared_mutex": "cpp"
-    }
-}

README.MD

@@ -1,10 +1,8 @@
 # OrbbecSDK ROS2 Wrapper v2
 
- [![stable](http://badges.github.io/stability-badges/dist/stable.svg)](http://github.com/badges/stability-badges) ![version](https://img.shields.io/badge/version-2.0.7-green)
-
 > [!IMPORTANT]
 >
-> Welcome to the OrbbecSDK ROS2 Wrapper v2. Before you begin using this version of ROS2 wrapper, it's crucial to check the following device support list to verify the compatibility.
+> Welcome to the OrbbecSDK ROS2 Wrapper v2. Before you begin using this version of ROS2 wrapper, it's crucial to check the following [device support list](#supported-devices) to verify the compatibility.
 
 OrbbecSDK ROS2 Wrapper provides seamless integration of Orbbec cameras with ROS 2 environment. It supports ROS2 Foxy, Humble, and Jazzy distributions.
 
@@ -125,6 +123,10 @@ Here is the device support list of main branch (v1.x) and v2-main branch (v2.x):
 4. not supported: we will not support specific device in this version;
 5. to be supported: we will add support in the near future.
 
+**Documentation Note:**
+
+The following instructions are only applicable to traditional launch files. If you want to experience our latest launch file format, please refer to the following link:
+
 ## Table of Contents
 
 - [OrbbecSDK ROS2 Wrapper v2](#orbbecsdk-ros2-wrapper-v2)
@@ -203,7 +205,8 @@ Install deb dependencies
 ```bash
 # assume you have sourced ROS environment, same blow
 sudo apt install libgflags-dev nlohmann-json3-dev  \
-ros-$ROS_DISTRO-image-transport ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
+ros-$ROS_DISTRO-image-transport ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
+ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
 ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
 ros-$ROS_DISTRO-backward-ros libdw-dev
 ```
@@ -353,12 +356,7 @@ ros2 launch orbbec_camera gemini_intra_process_demo_launch.py
 
 ## Use V4L2 backend
 
-To enable the V4L2 backend for the Gemini2 series cameras, follow these steps:
+[Setting uvc_backend](#launch-parameters)
-
-1. The Gemini2 series cameras support the V4L2 backend.
-2. Open the `config/OrbbecSDKConfig_v2.0.xml` file.
-3. Set the navigation option to `LinuxUVCBackend`.
-4. Change the backend setting to `V4L2`.
 
 Note: The V4L2 backend is not enabled by default.
 
@@ -370,6 +368,8 @@ The following are the launch parameters available:
   a longer time to initialize and reopening the device immediately can cause firmware crashes when hot plugging.
 - `enable_point_cloud`: Enables the point cloud.
 - `enable_colored_point_cloud`: Enables the RGB point cloud.
+- `cloud_frame_id`:Modifying the frame_id name within the ros message.
+- `ordered_pc`:Enable filtering of invalid point clouds.
 - `point_cloud_qos`, `[color|depth|ir]_qos`, `[color|depth|ir]_camera_info_qos`: ROS 2 Message Quality of Service (QoS)
   settings. The possible values
   are `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA` and are
@@ -378,6 +378,7 @@ The following are the launch parameters available:
   and `SENSOR_DATA`, respectively.
 - `enable_d2c_viewer`: Publishes the D2C overlay image (for testing only).
 - `device_num`: The number of devices. This must be filled in if multiple cameras are required.
+- `uvc_backend`:Optional values: v4l2, libuvc
 - `color_width`, `color_height`, `color_fps`: The resolution and frame rate of the color stream.
 - `ir_width`, `ir_height`, `ir_fps`: The resolution and frame rate of the IR stream.
 - `depth_width`, `depth_height`, `depth_fps`: The resolution and frame rate of the depth stream.
@@ -407,8 +408,6 @@ The following are the launch parameters available:
   filtering configuration file is located in the /config/depthfilter directory. Supported only on Gemini2.
 - `depth_precision`: The depth precision should be in the format `1mm`. The default value is `1mm`.
 - `enable_laser`: Enables the laser. The default value is `true`.
-- `laser_on_off_mode`: Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate. The default value
-  is `0`.
 - `device_preset`: The default value is `Default`. Only the G330 series is supported. For more information, refer to
   the [G330 documentation](https://www.orbbec.com/docs/g330-use-depth-presets/). Please refer to the table below to set
   the `device_preset` value based on your use case. The value should be one of the preset names
@@ -452,6 +451,7 @@ The following are the launch parameters available:
 - `interleave_frame_enable` : Whether to enable interleave frame mode.
 - `interleave_skip_enable` : Whether to enable skip frames.
 - `interleave_skip_index` : Set skip pattern IR or flood IR.
+- `[hdr|laser]_index[0|1]_[laser_control|depth_exposure|depth_gain|ir_brightness|ae_max_exposure]`:In interleave frame mode, set the 0th and 1st frame parameters of hdr or laser interleaving frames
 
 **IMPORTANT**: *Please carefully read the instructions regarding software filtering settings
 at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify
@@ -463,7 +463,7 @@ these settings.*
   * Imagine we are standing behind of the camera, and looking forward.
   * Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc..
 
-![ROS2 and Camera Coordinate System](docs/images/image7.png)
+![ROS2 and Camera Coordinate System](docs/source/image/image7.png)
 
 * ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up)
 * Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward)
@@ -472,9 +472,9 @@ these settings.*
 
 ## Camera sensor structure
 
-![module in rviz2](docs/images/image9.png)
+![module in rviz2](docs/source/image/image9.png)
 
-![module in rviz2](docs/images/image10.png)
+![module in rviz2](docs/source/image/image10.png)
 
 ## TF from coordinate A to coordinate B:
 
@@ -490,7 +490,7 @@ Example of static TFs of RGB sensor and right infra sensor of Gemini335 module a
 ros2 launch orbbec_description view_model.launch.py model:=gemini_335_336.urdf.xacro
 ```
 
-![module in rviz2](docs/images/image8.png)
+![module in rviz2](docs/source/image/image8.png)
 
 ## Predefined presets
 

docs/fastdds_tuning.md

@@ -0,0 +1,154 @@
+# Fast DDS Optimization for Orbbec Camera with ROS2
+
+When operating with the default configuration, Fast DDS exhibits suboptimal transmission efficiency, resulting in
+significant image transmission delays when used with the Orbbec camera in ROS2. This document provides guidance on
+optimizing Fast DDS to enhance image transfer efficiency.
+
+## 1. Adjusting System Parameters
+
+### IP Fragmentation Time
+
+- **Path**: `/proc/sys/net/ipv4/ipfrag_time` (default: 30 seconds)
+- **Purpose**: Defines the duration that IP fragments are kept in memory.
+- **Adjustment**: Decrease this value to reduce the time window where no fragments are received, which can help reduce
+  delays. Consider the specific needs of your environment as this setting affects all incoming fragments.
+
+  **Example**: Set to 3 seconds.
+
+  ```bash
+  sudo sysctl net.ipv4.ipfrag_time=3
+  ```
+
+### IP Fragmentation Memory Threshold
+
+- **Path**: `/proc/sys/net/ipv4/ipfrag_high_thresh` (default: 262144 bytes)
+- **Purpose**: Sets the maximum memory used to reassemble IP fragments.
+- **Adjustment**: Increase this value to allow more memory for fragment reassembly, which can improve handling of larger
+  data packets.
+
+  **Example**: Increase to 128 MB.
+
+  ```bash
+  sudo sysctl net.ipv4.ipfrag_high_thresh=134217728
+  ```
+
+### Maximum Buffer Sizes
+
+- **Purpose**: Configures the maximum buffer sizes for receiving and sending data, which is critical for high-throughput
+  data transmission.
+- **Adjustment**: Set the maximum buffer sizes for both receiving and sending operations.
+
+  **Commands**:
+
+  ```bash
+  sudo sysctl -w net.core.rmem_max=2147483647
+  sudo sysctl -w net.core.rmem_default=2147483647
+  sudo sysctl -w net.core.wmem_max=2147483647
+  sudo sysctl -w net.core.wmem_default=2147483647
+  ```
+
+Alternatively, make these settings permanent by adding them to the `/etc/sysctl.d/10-fastrtps-max.conf` file.
+
+```bash
+sudo gedit /etc/sysctl.d/10-fastrtps-max.conf
+```
+
+add blow lines to the file:
+
+```bash
+net.core.rmem_max=2147483647
+net.core.rmem_default=2147483647
+net.core.wmem_max=2147483647
+net.core.wmem_default=2147483647
+```
+
+then save and exit the file. run `sudo sysctl -p` to apply the changes.
+
+For detailed guidance, refer
+to [ROS 2 DDS Tuning Documentation](https://docs.ros.org/en/foxy/How-To-Guides/DDS-tuning.html).
+
+## 2. Fast DDS Configuration
+
+Below is an example of a Fast DDS configuration file optimized for ROS2 usage with the Orbbec camera. This configuration
+enhances the overall data transmission by adjusting buffer sizes and transport settings.
+
+### Configuration File: `shm_fastdds.xml`
+
+Place this file in the `$HOME` directory.
+
+```xml
+<?xml version="1.0" encoding="UTF-8"?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+    <transport_descriptors>
+        <transport_descriptor>
+            <transport_id>UDP_transport</transport_id>
+            <type>UDPv4</type>
+            <maxInitialPeersRange>10</maxInitialPeersRange>
+            <maxMessageSize>65000</maxMessageSize>
+            <sendBufferSize>1048576</sendBufferSize>
+            <receiveBufferSize>1048576</receiveBufferSize>
+        </transport_descriptor>
+    </transport_descriptors>
+    <participant profile_name="participant_profile_ros2" is_default_profile="true">
+        <rtps>
+            <name>profile_for_ros2_context</name>
+            <userTransports>
+                <transport_id>UDP_transport</transport_id>
+            </userTransports>
+            <useBuiltinTransports>false</useBuiltinTransports>
+            <sendSocketBufferSize>1048576</sendSocketBufferSize>
+            <listenSocketBufferSize>1048576</listenSocketBufferSize>
+            <builtin>
+                <initialPeersList>
+                    <locator>
+                        <udpv4>
+                            <address>127.0.0.1</address>
+                        </udpv4>
+                    </locator>
+                </initialPeersList>
+            </builtin>
+        </rtps>
+    </participant>
+    <data_writer profile_name="default publisher profile" is_default_profile="true">
+        <qos>
+            <publishMode>
+                <kind>ASYNCHRONOUS</kind>
+            </publishMode>
+            <latencyBudget>
+                <duration>
+                    <sec>0</sec>
+                    <nanosec>1000000</nanosec>
+                </duration>
+            </latencyBudget>
+        </qos>
+        <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    </data_writer>
+    <data_reader profile_name="default subscription profile" is_default_profile="true">
+        <qos>
+            <data_sharing>
+                <kind>AUTOMATIC</kind>
+            </data_sharing>
+            <latencyBudget>
+                <duration>
+                    <sec>0</sec>
+                    <nanosec>1000000</nanosec>
+                </duration>
+            </latencyBudget>
+        </qos>
+        <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    </data_reader>
+</profiles>
+```
+
+### Environment Variables
+
+Set the following environment variables to use the custom Fast DDS profile:
+
+```bash
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+export FASTRTPS_DEFAULT_PROFILES_FILE=$HOME/shm_fastdds.xml
+export RMW_FASTRTPS_USE_QOS_FROM_XML=1
+```
+
+This configuration aims to optimize the data flow and reduce transmission delays, improving the responsiveness and
+reliability of the Orbbec camera system in a ROS2 environment.

orbbec_camera/config/camera_params.yaml

@@ -1,14 +1,14 @@
 # common params
 depth_registration: false
+depth_registration: false
+enable_d2c_viewer: false
 enable_point_cloud: false
 enable_colored_point_cloud: false
 device_preset: "High Accuracy"
 laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on
 time_domain: "global" # global, device, system
-enable_sync_host_time: true
+enable_sync_host_time: false
 frames_per_trigger: 1
-log_level: "warning"
-enable_laser: false
 
 # When 3D reconstruction mode is enabled:
 # - The laser will switch to on-off mode
@@ -54,3 +54,5 @@ right_ir_height: 360
 right_ir_fps: 60
 right_ir_format: "Y8"
 right_ir_qos: "sensor_data"
+
+

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -390,7 +390,7 @@ class OBCameraNode {
   std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
   std::atomic_bool pipeline_started_{false};
   std::string camera_name_ = "camera";
-  const std::string imu_optical_frame_id_ = "camera_gyro_optical_frame";
+  std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame";
   const std::string imu_frame_id_ = "camera_gyro_frame";
   std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
   std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;

orbbec_camera/launch/gemini_330_series.launch.py

@@ -168,7 +168,7 @@ def generate_launch_description():
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_3d_reconstruction_mode', default_value='false'),
         DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
-        DeclareLaunchArgument('time_domain', default_value='device'),
+        DeclareLaunchArgument('time_domain', default_value='global'),
         DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),

orbbec_camera/src/ob_camera_node.cpp

@@ -1133,6 +1133,8 @@ void OBCameraNode::getParameters() {
     depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR];
   }
 
+  accel_gyro_frame_id_ = camera_name_ + "_accel_gyro_optical_frame";
+
   setAndGetNodeParameter(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro", false);
   for (const auto &stream_index : HID_STREAMS) {
     std::string param_name = stream_name_[stream_index] + "_qos";
@@ -1156,7 +1158,6 @@ void OBCameraNode::getParameters() {
     depth_aligned_frame_id_[stream_index] =
         camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame";
   }
-
   setAndGetNodeParameter(publish_tf_, "publish_tf", true);
   setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0);
   setAndGetNodeParameter(depth_registration_, "depth_registration", false);
@@ -2428,26 +2429,34 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   auto imu_msg = sensor_msgs::msg::Imu();
   setDefaultIMUMessage(imu_msg);
 
-  imu_msg.header.frame_id = imu_optical_frame_id_;
+  imu_msg.header.frame_id = optical_frame_id_[GYRO];
   auto frame_timestamp = getFrameTimestampUs(accelframe);
   auto timestamp = fromUsToROSTime(frame_timestamp);
   imu_msg.header.stamp = timestamp;
-  auto gyro_frame = gryoframe->as<ob::GyroFrame>();
+
   auto gyro_info = createIMUInfo(GYRO);
   gyro_info.header = imu_msg.header;
-  gyro_info.header.frame_id = imu_optical_frame_id_;
   imu_info_publishers_[GYRO]->publish(gyro_info);
+
+  auto accel_info = createIMUInfo(ACCEL);
+  imu_msg.header.frame_id = optical_frame_id_[ACCEL];
+  accel_info.header = imu_msg.header;
+  imu_info_publishers_[ACCEL]->publish(accel_info);
+
+  imu_msg.header.frame_id = accel_gyro_frame_id_;
+
+  auto gyro_frame = gryoframe->as<ob::GyroFrame>();
   auto gyroData = gyro_frame->getValue();
   imu_msg.angular_velocity.x = gyroData.x - gyro_info.bias[0];
   imu_msg.angular_velocity.y = gyroData.y - gyro_info.bias[1];
   imu_msg.angular_velocity.z = gyroData.z - gyro_info.bias[2];
+
   auto accel_frame = accelframe->as<ob::AccelFrame>();
   auto accelData = accel_frame->getValue();
-  auto accel_info = createIMUInfo(ACCEL);
   imu_msg.linear_acceleration.x = accelData.x - accel_info.bias[0];
   imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
   imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
-  imu_info_publishers_[ACCEL]->publish(accel_info);
+
   imu_gyro_accel_publisher_->publish(imu_msg);
 }
 
@@ -2469,14 +2478,15 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   }
   auto imu_msg = sensor_msgs::msg::Imu();
   setDefaultIMUMessage(imu_msg);
+
   imu_msg.header.frame_id = optical_frame_id_[stream_index];
   auto timestamp = fromUsToROSTime(frame->getTimeStampUs());
-
   imu_msg.header.stamp = timestamp;
+
   auto imu_info = createIMUInfo(stream_index);
   imu_info.header = imu_msg.header;
-  imu_info.header.frame_id = imu_optical_frame_id_;
   imu_info_publishers_[stream_index]->publish(imu_info);
+
   if (frame->getType() == OB_FRAME_GYRO) {
     auto gyro_frame = frame->as<ob::GyroFrame>();
     auto data = gyro_frame->getValue();
@@ -2501,7 +2511,7 @@ void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
   imu_msg.orientation.x = 0.0;
   imu_msg.orientation.y = 0.0;
   imu_msg.orientation.z = 0.0;
-  imu_msg.orientation.w = 0.0;
+  imu_msg.orientation.w = 1.0;
 
   imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   imu_msg.linear_acceleration_covariance = {
@@ -2885,7 +2895,7 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
   orbbec_camera_msgs::msg::IMUInfo imu_info;
   imu_info.header.frame_id = optical_frame_id_[stream_index];
   imu_info.header.stamp = node_->now();
-  auto imu_profile = stream_profile_[stream_index];
+
   if (stream_index == GYRO) {
     auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
     auto gyro_intrinsics = gyro_profile->getIntrinsic();

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -439,11 +439,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
 
   if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid())) {
     TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
-    sync_host_time_timer_ = this->create_wall_timer(std::chrono::seconds(60 * 60 * 2), [this]() {
-      if (device_) {
-        TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
-      }
-    });
   }
 
   RCLCPP_INFO_STREAM(logger_, "Device " << device_info_->getName() << " connected");