i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 03:30:18 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix DCL IMU synchronous output abnormal issue。

Browse Source
This commit is contained in:
lixiaobin
2024-01-10 16:42:25 +08:00
parent 51e77613f8
commit 573cf4e6e0
2 changed files with 140 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/launch/dabai_dcl.py
+135
View File
@@ -0,0 +1,135 @@
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
from launch_ros.actions import Node
import os
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='true'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='640'),
DeclareLaunchArgument('color_height', default_value='360'),
DeclareLaunchArgument('color_fps', default_value='15'),
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='15'),
DeclareLaunchArgument('depth_format', default_value='Y14'),
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('ir_width', default_value='640'),
DeclareLaunchArgument('ir_height', default_value='400'),
DeclareLaunchArgument('ir_fps', default_value='15'),
DeclareLaunchArgument('ir_format', default_value='Y8'),
DeclareLaunchArgument('enable_ir', default_value='true'),
DeclareLaunchArgument('flip_ir', default_value='false'),
DeclareLaunchArgument('ir_qos', default_value='default'),
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('enable_sync_output_accel_gyro', 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='500dps'),
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_ldp', default_value='true'),
# Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json
DeclareLaunchArgument('depth_filter_config', default_value=''),
# Depth work mode support is as follows:
# Unbinned Dense Default
# Unbinned Sparse Default
# Binned Sparse Default
# Obstacle Avoidance
DeclareLaunchArgument('depth_work_mode', default_value=''),
DeclareLaunchArgument('sync_mode', default_value='free_run'),
DeclareLaunchArgument('depth_delay_us', default_value='0'),
DeclareLaunchArgument('color_delay_us', default_value='0'),
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
]
# Node configuration
parameters = [{arg.name: LaunchConfiguration(arg.name)} for arg in args]
# get ROS_DISTRO
ros_distro = os.environ["ROS_DISTRO"]
if ros_distro == "foxy":
return LaunchDescription(
args
+ [
Node(
package="orbbec_camera",
executable="orbbec_camera_node",
name="ob_camera_node",
namespace=LaunchConfiguration("camera_name"),
parameters=parameters,
output="screen",
)
]
)
# Define the ComposableNode
else:
# 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
orbbec_camera/src/ob_camera_node.cpp
+5 -2
View File
@@ -351,11 +351,14 @@ void OBCameraNode::startIMU() {
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
imuConfig->enableStream(accelProfile);
imuConfig->enableStream(gyroProfile);
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame){
imuPipeline_->enableFrameSync();
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
auto frameSet = frame->as<ob::FrameSet>();
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
if (aFrame && gFrame) {
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
}
});
imu_sync_output_start_ = true;