diff --git a/.gitignore b/.gitignore index cb3d6897..f0ca0d45 100644 --- a/.gitignore +++ b/.gitignore @@ -764,6 +764,7 @@ FodyWeavers.xsd # JetBrains Rider *.sln.iml + ### VisualStudio Patch ### # Additional files built by Visual Studio diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 7c2789d0..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -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" - } -} \ No newline at end of file diff --git a/README.MD b/README.MD index 3894d1a4..ca5371e2 100644 --- a/README.MD +++ b/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: - -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`. +[Setting uvc_backend](#launch-parameters) 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 diff --git a/docs/fastdds_tuning.md b/docs/fastdds_tuning.md new file mode 100644 index 00000000..1e1ad021 --- /dev/null +++ b/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 + + + + + UDP_transport + UDPv4 + 10 + 65000 + 1048576 + 1048576 + + + + + profile_for_ros2_context + + UDP_transport + + false + 1048576 + 1048576 + + + + +
127.0.0.1
+ + + + + + + + + + ASYNCHRONOUS + + + + 0 + 1000000 + + + + PREALLOCATED_WITH_REALLOC + + + + + AUTOMATIC + + + + 0 + 1000000 + + + + PREALLOCATED_WITH_REALLOC + + +``` + +### 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. diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml index 47100b10..7a614316 100644 --- a/orbbec_camera/config/camera_params.yaml +++ b/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" + + diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 42c0303e..6c624aaa 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -390,7 +390,7 @@ class OBCameraNode { std::unique_ptr 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 pipeline_config_ = nullptr; std::map> sensors_; diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index e42bfb02..565205a9 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/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'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 24a5337f..99999475 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/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_ptras(); + 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(); 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(); 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 &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(); 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(); auto gyro_intrinsics = gyro_profile->getIntrinsic(); diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 7c31cf7b..562b4e6f 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -439,11 +439,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr &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");