mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 12:07:46 +08:00
1.更新orbbecSDK 1.9.1;2.gemini2增加深度滤波配置功能;3.增加IMU同时输出topic.
This commit is contained in:
lixiaobin
@@ -178,18 +178,37 @@ void OBCameraNode::setupDevices() {
|
||||
}
|
||||
}
|
||||
|
||||
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
|
||||
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
|
||||
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
||||
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
|
||||
if (!depth_filter_config_.empty() && enable_depth_filter_) {
|
||||
RCLCPP_INFO_STREAM(logger_, "Load depth filter config: " << depth_filter_config_);
|
||||
device_->loadDepthFilterConfig(depth_filter_config_.c_str());
|
||||
}
|
||||
auto default_soft_filter_speckle_size =
|
||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
||||
if (soft_filter_speckle_size_ != -1 &&
|
||||
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
|
||||
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_soft_filter_);
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_);
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
|
||||
device_->setBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
|
||||
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
|
||||
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
|
||||
}
|
||||
}
|
||||
|
||||
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
|
||||
auto default_soft_filter_speckle_size =
|
||||
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
|
||||
if (soft_filter_speckle_size_ != -1 &&
|
||||
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
|
||||
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, soft_filter_speckle_size_);
|
||||
}
|
||||
}
|
||||
} catch (const ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to setup devices: " << e.getMessage());
|
||||
@@ -296,49 +315,94 @@ void OBCameraNode::startStreams() {
|
||||
}
|
||||
|
||||
void OBCameraNode::startIMU() {
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||
CHECK(sensors_.count(stream_index));
|
||||
auto profile_list = sensors_[stream_index]->getStreamProfileList();
|
||||
for (size_t i = 0; i < profile_list->count(); i++) {
|
||||
auto item = profile_list->getProfile(i);
|
||||
if (stream_index == ACCEL) {
|
||||
auto profile = item->as<ob::AccelStreamProfile>();
|
||||
auto accel_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
|
||||
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
|
||||
sensors_[stream_index]->start(
|
||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||
onNewIMUFrameCallback(frame, stream_index);
|
||||
});
|
||||
imu_started_[stream_index] = true;
|
||||
RCLCPP_INFO_STREAM(logger_, "start accel stream with "
|
||||
<< magic_enum::enum_name(accel_range) << " range and "
|
||||
<< magic_enum::enum_name(accel_rate) << " rate");
|
||||
}
|
||||
} else if (stream_index == GYRO) {
|
||||
auto profile = item->as<ob::GyroStreamProfile>();
|
||||
auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
|
||||
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
|
||||
sensors_[stream_index]->start(
|
||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||
onNewIMUFrameCallback(frame, stream_index);
|
||||
});
|
||||
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
|
||||
<< magic_enum::enum_name(gyro_range) << " range and "
|
||||
<< magic_enum::enum_name(gyro_rate) << " rate");
|
||||
imu_started_[stream_index] = true;
|
||||
if (enable_sync_output_accel_gyro_) {
|
||||
if (imuPipeline_ != nullptr) {
|
||||
imuPipeline_.reset();
|
||||
}
|
||||
|
||||
imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
|
||||
if (imu_sync_output_start_){
|
||||
return;
|
||||
}
|
||||
|
||||
//ACCEL
|
||||
auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
|
||||
auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
|
||||
auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
|
||||
auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
|
||||
//GYRO
|
||||
auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
|
||||
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
|
||||
auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
|
||||
auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
|
||||
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){
|
||||
auto frameSet = frame->as<ob::FrameSet>();
|
||||
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
|
||||
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
|
||||
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
|
||||
});
|
||||
|
||||
imu_sync_output_start_ = true;
|
||||
if (!imu_sync_output_start_) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
logger_,
|
||||
"Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
|
||||
<< ",rate:" << sampleRateToString(accel_rate)
|
||||
<< ", and start gyro stream with range:"
|
||||
<< fullGyroScaleRangeToString(gyro_range)
|
||||
<< ",rate:" << sampleRateToString(gyro_rate));
|
||||
}
|
||||
} else {
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||
CHECK(sensors_.count(stream_index));
|
||||
auto profile_list = sensors_[stream_index]->getStreamProfileList();
|
||||
for (size_t i = 0; i < profile_list->count(); i++) {
|
||||
auto item = profile_list->getProfile(i);
|
||||
if (stream_index == ACCEL) {
|
||||
auto profile = item->as<ob::AccelStreamProfile>();
|
||||
auto accel_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
|
||||
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
|
||||
sensors_[stream_index]->start(
|
||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||
onNewIMUFrameCallback(frame, stream_index);
|
||||
});
|
||||
imu_started_[stream_index] = true;
|
||||
RCLCPP_INFO_STREAM(logger_, "start accel stream with "
|
||||
<< magic_enum::enum_name(accel_range) << " range and "
|
||||
<< magic_enum::enum_name(accel_rate) << " rate");
|
||||
}
|
||||
} else if (stream_index == GYRO) {
|
||||
auto profile = item->as<ob::GyroStreamProfile>();
|
||||
auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
|
||||
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
|
||||
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
|
||||
sensors_[stream_index]->start(
|
||||
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
|
||||
onNewIMUFrameCallback(frame, stream_index);
|
||||
});
|
||||
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
|
||||
<< magic_enum::enum_name(gyro_range) << " range and "
|
||||
<< magic_enum::enum_name(gyro_rate) << " rate");
|
||||
imu_started_[stream_index] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
|
||||
<< magic_enum::enum_name(stream_index.first)
|
||||
<< ", please check the imu_rate and imu_range parameters");
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
|
||||
<< magic_enum::enum_name(stream_index.first)
|
||||
<< ", please check the imu_rate and imu_range parameters");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -355,12 +419,23 @@ void OBCameraNode::stopStreams() {
|
||||
}
|
||||
|
||||
void OBCameraNode::stopIMU() {
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (imu_started_[stream_index]) {
|
||||
CHECK(sensors_.count(stream_index));
|
||||
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
|
||||
sensors_[stream_index]->stop();
|
||||
imu_started_[stream_index] = false;
|
||||
if (enable_sync_output_accel_gyro_) {
|
||||
if (!imu_sync_output_start_ || !imuPipeline_) {
|
||||
return;
|
||||
}
|
||||
try {
|
||||
imuPipeline_->stop();
|
||||
} catch (const ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Failed to stop imu pipeline: " << e.getMessage());
|
||||
}
|
||||
} else {
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (imu_started_[stream_index]) {
|
||||
CHECK(sensors_.count(stream_index));
|
||||
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
|
||||
sensors_[stream_index]->stop();
|
||||
imu_started_[stream_index] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -446,11 +521,15 @@ void OBCameraNode::getParameters() {
|
||||
depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR];
|
||||
}
|
||||
|
||||
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";
|
||||
setAndGetNodeParameter<std::string>(imu_qos_[stream_index], param_name, "default");
|
||||
param_name = "enable_" + stream_name_[stream_index];
|
||||
setAndGetNodeParameter(enable_stream_[stream_index], param_name, false);
|
||||
if(enable_sync_output_accel_gyro_) {
|
||||
enable_stream_[stream_index] = true;
|
||||
}
|
||||
param_name = stream_name_[stream_index] + "_rate";
|
||||
setAndGetNodeParameter<std::string>(imu_rate_[stream_index], param_name, "");
|
||||
param_name = stream_name_[stream_index] + "_range";
|
||||
@@ -477,6 +556,11 @@ void OBCameraNode::getParameters() {
|
||||
setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false);
|
||||
setAndGetNodeParameter(enable_hardware_d2d_, "enable_hardware_d2d", true);
|
||||
setAndGetNodeParameter(enable_soft_filter_, "enable_soft_filter", true);
|
||||
setAndGetNodeParameter<std::string>(depth_filter_config_, "depth_filter_config", "");
|
||||
if (!depth_filter_config_.empty()) {
|
||||
enable_soft_filter_ = false;
|
||||
enable_depth_filter_ = true;
|
||||
}
|
||||
setAndGetNodeParameter(enable_frame_sync_, "enable_frame_sync", false);
|
||||
setAndGetNodeParameter(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
|
||||
setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
|
||||
@@ -569,14 +653,22 @@ void OBCameraNode::setupPublishers() {
|
||||
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
|
||||
camera_info_qos_profile));
|
||||
}
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (!enable_stream_[stream_index]) {
|
||||
continue;
|
||||
}
|
||||
std::string data_topic_name = stream_name_[stream_index] + "/sample";
|
||||
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
|
||||
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
|
||||
|
||||
if (enable_sync_output_accel_gyro_) {
|
||||
std::string data_topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
|
||||
auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
|
||||
imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
|
||||
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
||||
} else {
|
||||
for (const auto &stream_index : HID_STREAMS) {
|
||||
if (!enable_stream_[stream_index]) {
|
||||
continue;
|
||||
}
|
||||
std::string data_topic_name = stream_name_[stream_index] + "/sample";
|
||||
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
|
||||
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
|
||||
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1102,6 +1194,38 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame> &accelframe,
|
||||
const std::shared_ptr<ob::Frame> &gryoframe) {
|
||||
if (!imu_gyro_accel_publisher_) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
|
||||
return;
|
||||
}
|
||||
auto subscriber_count = imu_gyro_accel_publisher_->get_subscription_count();
|
||||
if (subscriber_count == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto imu_msg = sensor_msgs::msg::Imu();
|
||||
setDefaultIMUMessage(imu_msg);
|
||||
|
||||
std::string imu_optical_frame_id ="camera_gyro_accel_optical_frame";
|
||||
imu_msg.header.frame_id = imu_optical_frame_id;
|
||||
auto timestamp = frameTimeStampToROSTime(accelframe->systemTimeStamp());
|
||||
imu_msg.header.stamp = timestamp;
|
||||
auto gyro_frame = gryoframe->as<ob::GyroFrame>();
|
||||
auto gyroData = gyro_frame->value();
|
||||
imu_msg.angular_velocity.x = gyroData.x;
|
||||
imu_msg.angular_velocity.y = gyroData.y;
|
||||
imu_msg.angular_velocity.z = gyroData.z;
|
||||
auto accel_frame = accelframe->as<ob::AccelFrame>();
|
||||
auto accelData = accel_frame->value();
|
||||
imu_msg.linear_acceleration.x = accelData.x;
|
||||
imu_msg.linear_acceleration.y = accelData.y;
|
||||
imu_msg.linear_acceleration.z = accelData.z;
|
||||
imu_gyro_accel_publisher_->publish(imu_msg);
|
||||
|
||||
}
|
||||
|
||||
void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
|
||||
const stream_index_pair &stream_index) {
|
||||
if (!imu_publishers_.count(stream_index)) {
|
||||
|
||||
Reference in New Issue
Block a user