i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merge to v2-main

Browse Source
This commit is contained in:
datean
2024-12-24 23:13:07 +08:00
parent e6c8d7ca5f 829f1d2ffd
commit 0d7f526d5d
9 changed files with 198 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/camera_params.yaml
+5 -3
View File
@@ -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
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+20 -10
View File
@@ -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
-5
View File
@@ -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");