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

refactor: update lidar log messages for clarity and consistency

Browse Source
This commit is contained in:
slz
2026-03-27 10:46:10 +08:00
parent 87f30c70b1
commit 26f20bf0ae
4 changed files with 44 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_lidar_node.h
-1
View File
@@ -252,7 +252,6 @@ class OBLidarNode {
std::string time_domain_ = "device"; // device, system, global
bool enable_scan_to_point_ = false;
bool enable_heartbeat_ = false;
bool enable_firmware_log_ = false;
bool use_intra_process_ = false;
// lidar
orbbec_camera/launch/lidar.launch.py
-5
View File
@@ -186,11 +186,6 @@ def generate_launch_description():
default_value='false',
description='Send heartbeat to device.'
),
DeclareLaunchArgument(
'enable_firmware_log',
default_value='false',
description='Enable device firmware log output.'
),
DeclareLaunchArgument(
'enable_imu',
default_value='false',
orbbec_camera/src/ob_camera_node_driver.cpp
+7 -7
View File
@@ -913,11 +913,11 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device net ip: " << net_ip);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device net ip: " << net_ip);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device net ip: " << net_ip);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device net ip: " << net_ip);
std::shared_ptr<ob::Device> device = nullptr;
for (size_t i = 0; i < list->getCount(); i++) {
try {
@@ -927,8 +927,8 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
if (list->getIpAddress(i) == nullptr) {
continue;
}
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
if (std::string(list->getIpAddress(i)) == net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"getDeviceByNetIP device net ip " << net_ip << " done");
@@ -965,7 +965,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
constexpr int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
RCLCPP_DEBUG_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
orbbec_camera/src/ob_lidar_node.cpp
+37 -39
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 color frame thread");
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);
@@ -131,7 +130,6 @@ void OBLidarNode::getParameters() {
setAndGetNodeParameter<double>(tf_publish_rate_, "tf_publish_rate", 0.0);
setAndGetNodeParameter<std::string>(time_domain_, "time_domain", "global");
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
setAndGetNodeParameter<bool>(enable_firmware_log_, "enable_firmware_log", false);
setAndGetNodeParameter<std::string>(echo_mode_, "echo_mode", "");
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
setAndGetNodeParameter<float>(min_angle_, "min_angle", -135.0);
@@ -182,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);
@@ -197,18 +197,15 @@ 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_);
}
device_->enableFirmwareLog(enable_firmware_log_);
RCLCPP_INFO_STREAM(logger_, "Set firmware log to " << (enable_firmware_log_ ? "ON" : "OFF"));
if (!echo_mode_.empty() &&
device_->isPropertySupported(OB_PROP_LIDAR_SPECIFIC_MODE_INT, OB_PERMISSION_READ_WRITE)) {
if (echo_mode_ == "Last Echo") {
@@ -217,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,
@@ -232,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));
}
}
@@ -245,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));
}
}
@@ -258,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));
}
}
@@ -332,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
@@ -502,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 {
@@ -520,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 {
@@ -545,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(
@@ -1275,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());
}
}