i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-03 19:47: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/launch/astra.launch.py
+1
View File
@@ -62,6 +62,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/dabai_a.launch.py
+1
View File
@@ -152,6 +152,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/dabai_al.launch.py
+1
View File
@@ -152,6 +152,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/dabai_dcw2.launch.py
+1
View File
@@ -72,6 +72,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
orbbec_camera/launch/dabai_max_pro.launch.py
+1
View File
@@ -66,6 +66,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini2.launch.py
+1
View File
@@ -84,6 +84,7 @@ def generate_launch_description():
DeclareLaunchArgument("ir_info_url", default_value=""),
DeclareLaunchArgument("color_info_url", default_value=""),
DeclareLaunchArgument("log_level", default_value="none"),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument("log_file_name", default_value=""),
DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
orbbec_camera/launch/gemini210.launch.py
+1
View File
@@ -84,6 +84,7 @@ def generate_launch_description():
DeclareLaunchArgument("ir_info_url", default_value=""),
DeclareLaunchArgument("color_info_url", default_value=""),
DeclareLaunchArgument("log_level", default_value="none"),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument("log_file_name", default_value=""),
DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
orbbec_camera/launch/gemini2L.launch.py
+1
View File
@@ -76,6 +76,7 @@ def generate_launch_description():
DeclareLaunchArgument("ir_info_url", default_value=""),
DeclareLaunchArgument("color_info_url", default_value=""),
DeclareLaunchArgument("log_level", default_value="none"),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument("log_file_name", default_value=""),
DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"),
DeclareLaunchArgument("enable_d2c_viewer", default_value="false"),
orbbec_camera/launch/gemini305.launch.py
+1
View File
@@ -214,6 +214,7 @@ def generate_launch_description():
DeclareLaunchArgument('device_access_mode', default_value='Default'), # Default, EA or CA . only for 335le
DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini345.launch.py
+1
View File
@@ -151,6 +151,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini345_lg.launch.py
+1
View File
@@ -152,6 +152,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini435_le.launch.py
+1
View File
@@ -182,6 +182,7 @@ def generate_launch_description():
DeclareLaunchArgument('net_device_port', default_value='0'),
DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini_330_series.launch.py
+1
View File
@@ -192,6 +192,7 @@ def generate_launch_description():
DeclareLaunchArgument('device_access_mode', default_value='Default'), # Default, EA or CA . only for 335le
DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini_330_series_low_cpu.launch.py
+1
View File
@@ -189,6 +189,7 @@ def generate_launch_description():
DeclareLaunchArgument('net_device_port', default_value='0'),
DeclareLaunchArgument('exposure_range_mode', default_value='default'),#default, ultimate or regular
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/gemini_intra_process_demo_launch.py
+1
View File
@@ -111,6 +111,7 @@ def generate_launch_description():
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('ros_log_level', default_value='info'),
DeclareLaunchArgument('log_file_name', default_value=''),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
orbbec_camera/launch/lidar.launch.py
+10
View File
@@ -171,6 +171,16 @@ def generate_launch_description():
default_value='none',
description='SDK log level: none, debug, info, warn, error, fatal.'
),
DeclareLaunchArgument(
'ros_log_level',
default_value='info',
description='ROS log level: debug, info, warn, error, fatal.'
),
DeclareLaunchArgument(
'log_file_name',
default_value='',
description='Custom log file name for SDK logs. If empty, default naming is used.'
),
DeclareLaunchArgument(
'time_domain',
default_value='device', # global, device, system
orbbec_camera/src/ob_camera_node.cpp
+238 -174
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/src/ob_camera_node_driver.cpp
+38 -10
View File
@@ -22,6 +22,7 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <rcutils/logging.h>
#include <csignal>
#include <sys/mman.h>
#include <unistd.h>
@@ -96,6 +97,24 @@ void signalHandler(int sig) {
namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh;
namespace {
int rosLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return RCUTILS_LOG_SEVERITY_DEBUG;
} else if (log_level == "info") {
return RCUTILS_LOG_SEVERITY_INFO;
} else if (log_level == "warn") {
return RCUTILS_LOG_SEVERITY_WARN;
} else if (log_level == "error") {
return RCUTILS_LOG_SEVERITY_ERROR;
} else if (log_level == "fatal") {
return RCUTILS_LOG_SEVERITY_FATAL;
}
return RCUTILS_LOG_SEVERITY_UNSET;
}
} // namespace
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
@@ -204,13 +223,21 @@ void OBCameraNodeDriver::init() {
ob::Context::setExtensionsDirectory(extension_path_.c_str());
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
auto log_level_str = declare_parameter<std::string>("log_level", "none");
auto ros_log_level_str = declare_parameter<std::string>("ros_log_level", "info");
auto log_level = obLogSeverityFromString(log_level_str);
auto ros_log_level = rosLogSeverityFromString(ros_log_level_str);
auto log_file_name = declare_parameter<std::string>("log_file_name", "");
std::string pwd_dir = std::getenv("PWD") ? std::getenv("PWD") : std::getenv("HOME");
std::string log_path = pwd_dir + "/Log/" + g_camera_name;
// Set logger to console
ob::Context::setLoggerToConsole(log_level);
ob::Context::setLoggerToFile(log_level, log_path.c_str());
if (ros_log_level != RCUTILS_LOG_SEVERITY_UNSET) {
auto ret = rcutils_logging_set_logger_level(this->get_logger().get_name(), ros_log_level);
if (ret != RCUTILS_RET_OK) {
RCLCPP_WARN_STREAM(logger_, "Failed to set ROS log level to " << ros_log_level_str);
}
}
// Set custom log file name if specified
if (!log_file_name.empty()) {
try {
@@ -289,7 +316,8 @@ void OBCameraNodeDriver::init() {
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
} else {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_,
"not support uvc_backend:" << uvc_backend_ << ", set to default libuvc");
}
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -885,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 {
@@ -899,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");
@@ -937,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 {
@@ -1283,7 +1311,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
// success get lock,break
break;
} else if (try_lock_result == EBUSY) {
RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
RCLCPP_WARN_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
std::this_thread::sleep_for(std::chrono::milliseconds(100));
} else {
RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_");
@@ -1309,7 +1337,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
}
auto end_time = std::chrono::high_resolution_clock::now();
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
RCLCPP_DEBUG_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
start_time = std::chrono::high_resolution_clock::now();
initializeDevice(device);
end_time = std::chrono::high_resolution_clock::now();
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());
}
}