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

Merge branch 'feature/log' into merge/sdk_2.8.2

Browse Source
This commit is contained in:
slz
2026-04-07 16:45:55 +08:00
parent 31baab180c 78f6ce7d05
commit 194de7e339
19 changed files with 338 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_lidar_node.cpp
+37 -36
View File
@@ -64,27 +64,26 @@ void OBLidarNode::setAndGetNodeParameter(
OBLidarNode::~OBLidarNode() noexcept { clean(); }
void OBLidarNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device");
RCLCPP_INFO_STREAM(logger_, "Rebooting device");
clean();
if (device_) {
device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE");
RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
}
}
void OBLidarNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBLidarNode");
RCLCPP_DEBUG_STREAM(logger_, "Destroying OBLidarNode");
is_running_.store(false);
RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join();
}
RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
RCLCPP_WARN_STREAM(logger_, "stop streams");
RCLCPP_DEBUG_STREAM(logger_, "Stop streams");
stopStreams();
stopIMU();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE");
RCLCPP_DEBUG_STREAM(logger_, "OBLidarNode cleanup complete");
}
void OBLidarNode::setupTopics() {
@@ -115,9 +114,9 @@ void OBLidarNode::getParameters() {
param_name = stream_name_[stream_index] + "_rate";
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
<< " Input format:"
<< magic_enum::enum_name(format_[stream_index]));
RCLCPP_DEBUG_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
<< " Input format:"
<< magic_enum::enum_name(format_[stream_index]));
param_name = stream_name_[stream_index] + "_frame_id";
std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
@@ -181,6 +180,8 @@ void OBLidarNode::getParameters() {
}
void OBLidarNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) {
auto sensor = sensor_list->getSensor(i);
@@ -196,14 +197,13 @@ void OBLidarNode::setupDevices() {
}
for (const auto &[stream_index, enable] : enable_stream_) {
if (enable && sensors_.find(stream_index) == sensors_.end()) {
RCLCPP_INFO_STREAM(logger_,
magic_enum::enum_name(stream_index.first)
<< "sensor isn't supported by current device! -- Skipping...");
RCLCPP_WARN_STREAM(logger_, magic_enum::enum_name(stream_index.first)
<< " sensor not supported by current device, skipping");
enable_stream_[stream_index] = false;
}
}
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF"));
RCLCPP_INFO_STREAM(logger_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
}
if (!echo_mode_.empty() &&
@@ -214,9 +214,9 @@ void OBLidarNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
}
RCLCPP_INFO_STREAM(
logger_, "Setting echo mode to "
<< (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT) ? "First Echo"
: "Last Echo"));
logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT)
? "First Echo"
: "Last Echo"));
}
if (repetitive_scan_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
@@ -229,7 +229,7 @@ void OBLidarNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
repetitive_scan_mode_);
RCLCPP_INFO_STREAM(logger_, "Setting repetitive scan mode to " << device_->getIntProperty(
RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
}
}
@@ -242,7 +242,7 @@ void OBLidarNode::setupDevices() {
} else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
RCLCPP_INFO_STREAM(logger_, "Setting filter level to " << device_->getIntProperty(
RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
}
}
@@ -255,7 +255,7 @@ void OBLidarNode::setupDevices() {
range.min, range.max);
} else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
RCLCPP_INFO_STREAM(logger_, "Setting vertical fov to " << device_->getFloatProperty(
RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
}
}
@@ -329,11 +329,11 @@ void OBLidarNode::setupProfiles() {
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()));
RCLCPP_DEBUG_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()));
}
}
// IMU
@@ -499,7 +499,7 @@ void OBLidarNode::startIMU() {
void OBLidarNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline");
RCLCPP_DEBUG_STREAM(logger_, "Pipeline not started or not exist, skip stop pipeline");
return;
}
try {
@@ -517,7 +517,7 @@ void OBLidarNode::stopIMU() {
}
if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_INFO_STREAM(logger_, "IMU pipeline not started or not exist, skip stop imu pipeline");
RCLCPP_DEBUG_STREAM(logger_, "IMU pipeline not started or not exist, skip stop imu pipeline");
return;
}
try {
@@ -542,10 +542,10 @@ void OBLidarNode::setupPipelineConfig() {
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()));
RCLCPP_DEBUG_STREAM(logger_,
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
<< " "
<< magic_enum::enum_name(video_profile->getFormat()));
}
RCLCPP_INFO_STREAM(
@@ -1272,11 +1272,12 @@ void OBLidarNode::calcAndPublishStaticTransform() {
auto timestamp = node_->now();
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from "
<< frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
RCLCPP_DEBUG_STREAM(logger_, "Publishing static transform from "
<< frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
RCLCPP_DEBUG_STREAM(logger_,
"Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_DEBUG_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
}
}