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

Update setupTopics

Browse Source
This commit is contained in:
jj
2025-06-13 17:30:05 +08:00
parent b365727f0a
commit 2be9c2d021
2 changed files with 93 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_lidar_node.h
+1 -1
View File
@@ -122,7 +122,7 @@ const stream_index_pair LIDAR{OB_STREAM_LIDAR, 0};
const stream_index_pair GYRO{OB_STREAM_GYRO, 0}; const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0}; const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
const std::vector<stream_index_pair> IMAGE_STREAMS = {LIDAR}; const std::vector<stream_index_pair> LIDAR_STREAMS = {LIDAR};
const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL}; const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
orbbec_camera/src/ob_lidar_node.cpp
+92 -91
View File
@@ -116,16 +116,16 @@ void OBLidarNode::setupTopics() {
void OBLidarNode::getParameters() { void OBLidarNode::getParameters() {
setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "lidar"); setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "lidar");
for (auto stream_index : IMAGE_STREAMS) { for (auto stream_index : LIDAR_STREAMS) {
std::string param_name = stream_name_[stream_index] + "_format"; std::string param_name = stream_name_[stream_index] + "_format";
setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]); setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]);
format_[stream_index] = OBFormatFromString(format_str_[stream_index]); format_[stream_index] = OBFormatFromString(format_str_[stream_index]);
param_name = stream_name_[stream_index] + "_rate"; param_name = stream_name_[stream_index] + "_rate";
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0); setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]); rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
logger_, "Input rate: " << magic_enum::enum_name(rate_[LIDAR]) << " Input format:"
<< " Input format:" << magic_enum::enum_name(format_[LIDAR])); << magic_enum::enum_name(format_[stream_index]));
param_name = stream_name_[stream_index] + "_frame_id"; param_name = stream_name_[stream_index] + "_frame_id";
std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame"; std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id); setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
@@ -171,84 +171,87 @@ void OBLidarNode::setupDevices() {
} else if (echo_mode_ == "First Echo") { } else if (echo_mode_ == "First Echo") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_ECHO_MODE_INT, 1); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_ECHO_MODE_INT, 1);
} }
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(logger_,
logger_, "Setting echo mode to " "Setting echo mode to "
<< (device_->getIntProperty(OB_PROP_LIDAR_ECHO_MODE_INT) ? "single channel" << (device_->getIntProperty(OB_PROP_LIDAR_ECHO_MODE_INT) ? "First Echo"
: "dual channel")); : "Last Echo"));
} }
} }
void OBLidarNode::setupProfiles() { void OBLidarNode::setupProfiles() {
if (enable_stream_[LIDAR]) { for (const auto &elem : LIDAR_STREAMS) {
const auto &sensor = sensors_[LIDAR]; if (enable_stream_[elem]) {
CHECK_NOTNULL(sensor.get()); const auto &sensor = sensors_[elem];
auto profiles = sensor->getStreamProfileList(); CHECK_NOTNULL(sensor.get());
CHECK_NOTNULL(profiles.get()); auto profiles = sensor->getStreamProfileList();
CHECK(profiles->getCount() > 0); CHECK_NOTNULL(profiles.get());
for (size_t i = 0; i < profiles->getCount(); i++) { CHECK(profiles->getCount() > 0);
auto base_profile = profiles->getProfile(i)->as<ob::LiDARStreamProfile>(); for (size_t i = 0; i < profiles->getCount(); i++) {
if (base_profile == nullptr) { auto base_profile = profiles->getProfile(i)->as<ob::LiDARStreamProfile>();
throw std::runtime_error("Failed to get profile " + std::to_string(i)); if (base_profile == nullptr) {
} throw std::runtime_error("Failed to get profile " + std::to_string(i));
auto profile = base_profile->as<ob::LiDARStreamProfile>(); }
if (profile == nullptr) { auto profile = base_profile->as<ob::LiDARStreamProfile>();
throw std::runtime_error("Failed cast profile to LiDARStreamProfile"); if (profile == nullptr) {
} throw std::runtime_error("Failed cast profile to LiDARStreamProfile");
RCLCPP_DEBUG_STREAM( }
logger_, RCLCPP_DEBUG_STREAM(
"Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->getType()) logger_,
<< "Scan Rate: " << magic_enum::enum_name(profile->getScanRate()) "Sensor profile: " << "stream_type: " << magic_enum::enum_name(profile->getType())
<< "Format:" << magic_enum::enum_name(profile->getFormat())); << "Scan Rate: " << magic_enum::enum_name(profile->getScanRate())
supported_profiles_[LIDAR].emplace_back(profile); << "Format:" << magic_enum::enum_name(profile->getFormat()));
} supported_profiles_[elem].emplace_back(profile);
std::shared_ptr<ob::LiDARStreamProfile> selected_profile;
std::shared_ptr<ob::LiDARStreamProfile> default_profile;
try {
if (rate_[LIDAR] == OB_LIDAR_SCAN_UNKNOWN && format_[LIDAR] == OB_FORMAT_UNKNOWN) {
selected_profile = profiles->getProfile(0)->as<ob::LiDARStreamProfile>();
} else {
selected_profile = profiles->getLiDARStreamProfile(rate_[LIDAR], format_[LIDAR]);
} }
std::shared_ptr<ob::LiDARStreamProfile> selected_profile;
std::shared_ptr<ob::LiDARStreamProfile> default_profile;
try {
if (rate_[elem] == OB_LIDAR_SCAN_UNKNOWN && format_[elem] == OB_FORMAT_UNKNOWN) {
selected_profile = profiles->getProfile(0)->as<ob::LiDARStreamProfile>();
} else {
selected_profile = profiles->getLiDARStreamProfile(rate_[elem], format_[elem]);
}
} catch (const ob::Error &ex) { } catch (const ob::Error &ex) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to get " << stream_name_[LIDAR] << " profile: " << ex.getMessage());
RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(LIDAR.first)
<< ", Stream Index: " << LIDAR.second
<< ", Scan Rate: " << rate_[LIDAR]
<< "Format:" << format_[LIDAR]);
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
exit(-1);
}
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(LIDAR.first)
<< ", Stream Index: " << LIDAR.second
<< ", Scan Rate: " << rate_[LIDAR]);
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default scan Rate "
<< magic_enum::enum_name(default_profile->getScanRate())
<< "default format:"
<< magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM( RCLCPP_ERROR_STREAM(
logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(LIDAR.first) logger_, "Failed to get " << stream_name_[elem] << " profile: " << ex.getMessage());
<< " will be disable"); RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(elem.first)
enable_stream_[LIDAR] = false; << ", Stream Index: " << elem.second
<< ", Scan Rate: " << rate_[elem]
<< "Format:" << format_[elem]);
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit.");
exit(-1);
} }
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second
<< ", Scan Rate: " << rate_[elem]);
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default scan Rate "
<< magic_enum::enum_name(default_profile->getScanRate())
<< "default format:"
<< magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(
logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(elem.first)
<< " will be disable");
enable_stream_[elem] = false;
}
}
CHECK_NOTNULL(selected_profile);
stream_profile_[elem] = selected_profile;
rate_[elem] = selected_profile->getScanRate();
format_[elem] = selected_profile->getFormat();
RCLCPP_INFO_STREAM(logger_, " stream "
<< stream_name_[elem] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate())
<< " format:"
<< magic_enum::enum_name(selected_profile->getFormat()));
} }
CHECK_NOTNULL(selected_profile);
stream_profile_[LIDAR] = selected_profile;
rate_[LIDAR] = selected_profile->getScanRate();
format_[LIDAR] = selected_profile->getFormat();
RCLCPP_INFO_STREAM(
logger_, " stream " << stream_name_[LIDAR] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate())
<< " format:" << magic_enum::enum_name(selected_profile->getFormat()));
} }
} }
@@ -303,8 +306,6 @@ void OBLidarNode::startStreams() {
}); });
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage()); RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
enable_stream_[LIDAR] = false;
setupPipelineConfig(); setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) { pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set); onNewFrameSetCallback(frame_set);
@@ -335,22 +336,25 @@ void OBLidarNode::setupPipelineConfig() {
pipeline_config_.reset(); pipeline_config_.reset();
} }
pipeline_config_ = std::make_shared<ob::Config>(); pipeline_config_ = std::make_shared<ob::Config>();
if (enable_stream_[LIDAR]) { for (const auto &stream_index : LIDAR_STREAMS) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[LIDAR] << " stream"); if (enable_stream_[stream_index]) {
auto profile = stream_profile_[LIDAR]->as<ob::LiDARStreamProfile>(); RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
auto profile = stream_profile_[stream_index]->as<ob::LiDARStreamProfile>();
if (enable_stream_[stream_index]) {
auto video_profile = profile;
RCLCPP_INFO_STREAM(logger_,
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
<< " "
<< magic_enum::enum_name(video_profile->getFormat()));
}
if (enable_stream_[LIDAR]) {
auto video_profile = profile;
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
logger_, "lidar profile: " << magic_enum::enum_name(video_profile->getScanRate()) << " " logger_, "Stream " << stream_name_[stream_index]
<< magic_enum::enum_name(video_profile->getFormat())); << " scan rate: " << magic_enum::enum_name(profile->getScanRate())
<< " format: " << magic_enum::enum_name(profile->getFormat()));
pipeline_config_->enableStream(stream_profile_[stream_index]);
} }
RCLCPP_INFO_STREAM(logger_,
"Stream " << stream_name_[LIDAR]
<< " scan rate: " << magic_enum::enum_name(profile->getScanRate())
<< " format: " << magic_enum::enum_name(profile->getFormat()));
pipeline_config_->enableStream(stream_profile_[LIDAR]);
} }
} }
@@ -690,9 +694,6 @@ void OBLidarNode::calcAndPublishStaticTransform() {
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]); tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
auto timestamp = node_->now(); auto timestamp = node_->now();
if (stream_index.first != base_stream_.first) { if (stream_index.first != base_stream_.first) {
if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) {
trans[0] = std::abs(trans[0]); // because left and right ir calibration is error
}
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]); publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]);
} }
publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index], publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],