Merge branch 'main' of https://github.com/AIResearchLab/OrbbecSDK_ROS2

README.MD

@@ -34,6 +34,8 @@ supports ROS 2 Foxy, Humble, and Jazzy distributions.
   * [Product support](#product-support)
   * [DDS Tuning](#dds-tuning)
   * [Frequently Asked Questions](#frequently-asked-questions)
+    * [No Data Stream from Multiple Cameras](#no-data-stream-from-multiple-cameras)
+    * [Why Are There So Many Launch Files Here?](#why-are-there-so-many-launch-files-here)
   * [Other useful links](#other-useful-links)
   * [License](#license)
 <!-- TOC -->
@@ -303,6 +305,14 @@ The following are the launch parameters available:
 
 - `enable_3d_reconstruction_mode`: Enables 3D reconstruction mode. Default is `false`. When set to `true`, the camera 
 laser operates in on-off mode, capturing IR images (laser off) for VSLAM localization and depth images (laser on) for point cloud computation.
+- `tf_publish_rate`: The rate at which the camera publishes dynamic transforms. The default value is `0.0`, which means static transforms are published.
+- `time_domain`: The frame time domain, string type, can be `device`, `global`, or `system`. `device` means using the hardware timestamp from the camera,
+`system` means using the timestamp when the PC received the first packet of data or frame, and `global` is used for synchronized time across multiple
+devices, aligning data from different sources to a common time base.
+- `enable_sync_host_time`: Enables synchronization of the host time with the camera time. The default value is `true`, if
+use global time, set to `false`. Some old devices may not support this feature.
+- `config_file_path`: The path to the YAML configuration file. The default value is `""`. If the configuration file is not specified,
+the default parameters from the launch file will be used. If you want to use a custom configuration file, please refer to `gemini_330_series.launch.py`.
 
 **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
@@ -671,21 +681,29 @@ net.core.rmem_max=2147483647
 net.core.rmem_default=2147483647
 ```
 
+If you use Fast DDS, you can refer to the [Fast DDS Configuration](./docs/fastdds_tuning.md) file.
+
 ## Frequently Asked Questions
 
-No Picture from Multiple Cameras
+### No Data Stream from Multiple Cameras
 
-- it's possible that the power supply is insufficient.
-  To avoid this, do not connect all cameras to the same hub and use a powered hub instead.
+**Insufficient Power Supply**:
+  - Ensure that all cameras are not connected to the same hub.
+  - Use a powered hub to provide sufficient power to each camera.
+**High Resolution**:
+  - Try lowering the resolution to resolve data stream issues.
 
-- It's also possible that the resolution is too high.
-  To resolve this, try lowering the resolution.
+**Increase usbfs_memory_mb Value**:
+  - Increase the `usbfs_memory_mb` value to 128MB by running the following command:
+    ```bash
+    echo 128 | sudo tee /sys/module/usbcore/parameters/usbfs_memory_mb
+    ```
+  - For making this change permanent, check [this link](https://github.com/OpenKinect/libfreenect2/issues/807).
 
-Why are there so many launch files here
+### Why Are There So Many Launch Files Here?
 
-- The reason for the presence of multiple launch
-  files is due to the fact that the default resolutions and image formats of different cameras vary.
-  To make it easier to use, the launch files have been separated for each camera.
+- Multiple launch files exist because different cameras have varying default resolutions and image formats.
+- To simplify usage, each camera has its own launch file.
 
 ## Other useful links
 

docs/fastdds_tuning.md

@@ -0,0 +1,159 @@
+Here's a professional, English version of the document you provided for your client regarding tuning Fast DDS for Orbbec
+camera usage with ROS2:
+
+---
+
+# 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/SDK/include/libobsensor/h/ObTypes.h

@@ -1440,6 +1440,14 @@ typedef struct {
     int16_t y1_bottom;
 } AE_ROI, ob_region_of_interest, OBRegionOfInterest;
 
+
+typedef struct{
+  uint8_t enable;
+  uint8_t offset0;
+  uint8_t offset1;
+  uint8_t reserved;
+}DISP_OFFSET_CONFIG,ob_disp_offset_config,OBDispOffsetConfig;
+
 /**
  * @brief Frame metadata types
  * @brief The frame metadata is a set of meta info generated by the device for current individual frame.

orbbec_camera/SDK/lib/arm32/libOrbbecSDK.so.1.10

@@ -1 +1 @@
-libOrbbecSDK.so.1.10.9
+libOrbbecSDK.so.1.10.11

orbbec_camera/SDK/lib/arm64/libOrbbecSDK.so.1.10

@@ -1 +1 @@
-libOrbbecSDK.so.1.10.9
+libOrbbecSDK.so.1.10.11

orbbec_camera/SDK/lib/x64/libOrbbecSDK.so.1.10

@@ -1 +1 @@
-libOrbbecSDK.so.1.10.9
+libOrbbecSDK.so.1.10.11

orbbec_camera/config/camera_params.yaml

@@ -4,6 +4,9 @@ enable_point_cloud: false
 enable_colored_point_cloud: false
 device_preset: "High Accuracy"
 laser_on_off_mode: 1 # 0: off, 1: on-off, 1: off-on
+time_domain: "global" # global, device, system
+enable_sync_host_time: false
+frames_per_trigger: 2
 
 # When 3D reconstruction mode is enabled:
 # - The laser will switch to on-off mode
@@ -15,7 +18,7 @@ enable_3d_reconstruction_mode: true
 enable_color: true
 color_width: 640
 color_height: 480
-color_fps: 60
+color_fps: 90
 color_format: "YUYV"
 enable_color_auto_exposure: false
 color_exposure: 50 # 5ms
@@ -24,7 +27,7 @@ color_gain: -1 # -1 default
 # depth params
 depth_width: 640
 depth_height: 480
-depth_fps: 60
+depth_fps: 90
 depth_format: "Y16"
 
 # ir exposure
@@ -36,12 +39,12 @@ ir_gain: 40
 enable_left_ir: true
 left_ir_width: 640
 left_ir_height: 480
-left_ir_fps: 60
+left_ir_fps: 90
 left_ir_format: "Y8"
 
 #right ir params
 enable_right_ir: true
 right_ir_width: 640
 right_ir_height: 480
-right_ir_fps: 60
+right_ir_fps: 90
 right_ir_format: "Y8"

orbbec_camera/include/orbbec_camera/constants.h

@@ -23,7 +23,7 @@
 
 #define OB_ROS_MAJOR_VERSION 1
 #define OB_ROS_MINOR_VERSION 5
-#define OB_ROS_PATCH_VERSION 8
+#define OB_ROS_PATCH_VERSION 10
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -270,7 +270,7 @@ class OBCameraNode {
                             std::shared_ptr<GetBool::Response>& response);
 
   void getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
-                          std::shared_ptr<GetInt32::Response>& response);
+                                     std::shared_ptr<GetInt32::Response>& response);
 
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
@@ -294,6 +294,8 @@ class OBCameraNode {
 
   std::shared_ptr<ob::Frame> processDepthFrameFilter(std::shared_ptr<ob::Frame>& frame);
 
+  uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame> & frame);
+
   void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
 
   std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
@@ -471,6 +473,7 @@ class OBCameraNode {
   int trigger2image_delay_us_ = 0;
   int trigger_out_delay_us_ = 0;
   bool trigger_out_enabled_ = false;
+  int frames_per_trigger_ = 2;
   std::string depth_precision_str_;
   OB_DEPTH_PRECISION_LEVEL depth_precision_ = OB_PRECISION_0MM8;
   double depth_precision_float_ = 0.10;
@@ -498,7 +501,6 @@ class OBCameraNode {
   std::condition_variable color_frame_queue_cv_;
 
   bool ordered_pc_ = false;
-  bool use_hardware_time_ = true;
   bool enable_depth_scale_ = true;
   std::shared_ptr<ob::Frame> depth_frame_ = nullptr;
   std::string device_preset_ = "Default";
@@ -550,5 +552,11 @@ class OBCameraNode {
   uint8_t* rgb_point_cloud_buffer_ = nullptr;
   uint32_t rgb_point_cloud_buffer_size_ = 0;
   bool enable_3d_reconstruction_mode_ = false;
+  int min_depth_limit_ = 0;
+  int max_depth_limit_ = 0;
+  std::string time_domain_ = "device"; // device, system, global
+  // soft ware trigger
+  rclcpp::TimerBase::SharedPtr software_trigger_timer_;
+  std::chrono::milliseconds software_trigger_period_{33};
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -97,5 +97,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::string net_device_ip_;
   int net_device_port_ = 0;
   int connection_delay_ = 100;
+  bool enable_sync_host_time_ = true;
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/astra.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         DeclareLaunchArgument('ir_exposure', default_value='-1'),
         DeclareLaunchArgument('ir_gain', default_value='-1'),
         DeclareLaunchArgument('publish_tf', default_value='true'),
-        DeclareLaunchArgument('tf_publish_rate', default_value='10.0'),
+        DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
         DeclareLaunchArgument('ir_info_url', default_value=''),
         DeclareLaunchArgument('color_info_url', default_value=''),
         DeclareLaunchArgument('log_level', default_value='none'),

orbbec_camera/launch/astra2.launch.py

@@ -64,7 +64,7 @@ def generate_launch_description():
         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('tf_publish_rate', default_value='0.0'),
         DeclareLaunchArgument('ir_info_url', default_value=''),
         DeclareLaunchArgument('color_info_url', default_value=''),
         DeclareLaunchArgument('log_level', default_value='none'),

orbbec_camera/launch/astra_adv.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         DeclareLaunchArgument('ir_exposure', default_value='-1'),
         DeclareLaunchArgument('ir_gain', default_value='-1'),
         DeclareLaunchArgument('publish_tf', default_value='true'),
-        DeclareLaunchArgument('tf_publish_rate', default_value='10.0'),
+        DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
         DeclareLaunchArgument('ir_info_url', default_value=''),
         DeclareLaunchArgument('color_info_url', default_value=''),
         DeclareLaunchArgument('log_level', default_value='none'),

orbbec_camera/launch/astra_embedded_s.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         DeclareLaunchArgument('ir_exposure', default_value='-1'),
         DeclareLaunchArgument('ir_gain', default_value='-1'),
         DeclareLaunchArgument('publish_tf', default_value='true'),
-        DeclareLaunchArgument('tf_publish_rate', default_value='10.0'),
+        DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
         DeclareLaunchArgument('ir_info_url', default_value=''),
         DeclareLaunchArgument('color_info_url', default_value=''),
         DeclareLaunchArgument('log_level', default_value='none'),