mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-07 21:37:46 +08:00
1387 lines
61 KiB
C++
1387 lines
61 KiB
C++
/*******************************************************************************
|
|
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*******************************************************************************/
|
|
|
|
#include "orbbec_camera/ob_lidar_node.h"
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <thread>
|
|
#include <geometry_msgs/msg/transform_stamped.hpp>
|
|
|
|
#include "orbbec_camera/utils.h"
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
|
|
#include "libobsensor/hpp/Utils.hpp"
|
|
|
|
namespace orbbec_camera {
|
|
namespace orbbec_lidar {
|
|
using namespace std::chrono_literals;
|
|
|
|
OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
|
|
std::shared_ptr<Parameters> parameters, bool use_intra_process)
|
|
: node_(node),
|
|
device_(std::move(device)),
|
|
parameters_(std::move(parameters)),
|
|
logger_(node->get_logger()),
|
|
use_intra_process_(use_intra_process) {
|
|
RCLCPP_INFO_STREAM(logger_,
|
|
"OBLidarNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
|
|
is_running_.store(true);
|
|
stream_name_[LIDAR] = "lidar";
|
|
stream_name_[ACCEL] = "accel";
|
|
stream_name_[GYRO] = "gyro";
|
|
setupTopics();
|
|
is_camera_node_initialized_ = true;
|
|
}
|
|
|
|
template <class T>
|
|
void OBLidarNode::setAndGetNodeParameter(
|
|
T ¶m, const std::string ¶m_name, const T &default_value,
|
|
const rcl_interfaces::msg::ParameterDescriptor ¶meter_descriptor) {
|
|
try {
|
|
param = parameters_
|
|
->setParam(param_name, rclcpp::ParameterValue(default_value),
|
|
std::function<void(const rclcpp::Parameter &)>(), parameter_descriptor)
|
|
.get<T>();
|
|
} catch (const rclcpp::ParameterTypeException &ex) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to set parameter: " << param_name << ". " << ex.what());
|
|
throw;
|
|
}
|
|
}
|
|
|
|
OBLidarNode::~OBLidarNode() noexcept { clean(); }
|
|
|
|
void OBLidarNode::rebootDevice() {
|
|
RCLCPP_INFO_STREAM(logger_, "Rebooting device");
|
|
clean();
|
|
if (device_) {
|
|
device_->reboot();
|
|
RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::clean() noexcept {
|
|
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
|
|
RCLCPP_DEBUG_STREAM(logger_, "Destroying OBLidarNode");
|
|
is_running_.store(false);
|
|
RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
|
|
if (tf_thread_ && tf_thread_->joinable()) {
|
|
tf_thread_->join();
|
|
}
|
|
RCLCPP_DEBUG_STREAM(logger_, "Stop streams");
|
|
stopStreams();
|
|
stopIMU();
|
|
RCLCPP_DEBUG_STREAM(logger_, "OBLidarNode cleanup complete");
|
|
}
|
|
|
|
void OBLidarNode::setupTopics() {
|
|
try {
|
|
getParameters();
|
|
setupDevices();
|
|
selectBaseStream();
|
|
setupProfiles();
|
|
setupPublishers();
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_ERROR_STREAM(logger_,
|
|
"Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
|
|
throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
|
|
} catch (const std::exception &e) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
|
|
throw std::runtime_error(e.what());
|
|
} catch (...) {
|
|
RCLCPP_ERROR(logger_, "Failed to setup topics");
|
|
throw std::runtime_error("Failed to setup topics");
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::getParameters() {
|
|
setAndGetNodeParameter<std::string>(camera_name_, "camera_name", "lidar");
|
|
for (auto stream_index : LIDAR_STREAMS) {
|
|
std::string param_name = stream_name_[stream_index] + "_format";
|
|
setAndGetNodeParameter(format_str_[stream_index], param_name, format_str_[stream_index]);
|
|
format_[stream_index] = OBFormatFromString(format_str_[stream_index]);
|
|
param_name = stream_name_[stream_index] + "_rate";
|
|
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
|
|
rate_[stream_index] = OBScanRateFromInt(rate_int_[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);
|
|
std::string default_optical_frame_id =
|
|
camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
|
|
param_name = stream_name_[stream_index] + "_optical_frame_id";
|
|
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
|
|
}
|
|
setAndGetNodeParameter<bool>(enable_scan_to_point_, "enable_scan_to_point", false);
|
|
setAndGetNodeParameter<bool>(publish_tf_, "publish_tf", true);
|
|
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<std::string>(echo_mode_, "echo_mode", "");
|
|
setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
|
|
setAndGetNodeParameter<float>(min_angle_, "min_angle", -135.0);
|
|
setAndGetNodeParameter<float>(max_angle_, "max_angle", 135.0);
|
|
setAndGetNodeParameter<float>(min_range_, "min_range", 0.05);
|
|
setAndGetNodeParameter<float>(max_range_, "max_range", 30.0);
|
|
setAndGetNodeParameter<int>(repetitive_scan_mode_, "repetitive_scan_mode", -1);
|
|
setAndGetNodeParameter<int>(filter_level_, "filter_level", -1);
|
|
setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
|
|
setAndGetNodeParameter<bool>(enable_imu_, "enable_imu", false);
|
|
setAndGetNodeParameter<std::string>(imu_rate_, "imu_rate", "50hz");
|
|
setAndGetNodeParameter<std::string>(accel_range_, "accel_range", "2g");
|
|
setAndGetNodeParameter<std::string>(gyro_range_, "gyro_range", "1000dps");
|
|
setAndGetNodeParameter<std::string>(imu_qos_, "imu_qos", "default");
|
|
setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
|
|
setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
|
|
|
|
// Multi-frame publishing parameter - only for LIDAR_POINT and LIDAR_SPHERE_POINT formats
|
|
setAndGetNodeParameter<int>(publish_n_pkts_, "publish_n_pkts", 1);
|
|
if (publish_n_pkts_ < 1 || publish_n_pkts_ > 12000) {
|
|
RCLCPP_WARN_STREAM(logger_, "publish_n_pkts value "
|
|
<< publish_n_pkts_
|
|
<< " is out of range [1, 12000], setting to 1");
|
|
publish_n_pkts_ = 1;
|
|
}
|
|
if (publish_n_pkts_ > 1)
|
|
RCLCPP_INFO_STREAM(
|
|
logger_, "Multi-frame publishing enabled: " << publish_n_pkts_ << " frames will be merged");
|
|
|
|
// Setup IMU streams if enabled
|
|
if (enable_imu_) {
|
|
enable_stream_[ACCEL] = true;
|
|
enable_stream_[GYRO] = true;
|
|
|
|
for (const auto &stream_index : HID_STREAMS) {
|
|
std::string param_name = camera_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);
|
|
std::string default_optical_frame_id =
|
|
camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame";
|
|
param_name = stream_name_[stream_index] + "_optical_frame_id";
|
|
setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id);
|
|
}
|
|
|
|
// Set unified IMU frame ID
|
|
accel_gyro_frame_id_ = camera_name_ + "_imu_frame";
|
|
}
|
|
}
|
|
|
|
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);
|
|
auto profiles = sensor->getStreamProfileList();
|
|
for (size_t j = 0; j < profiles->getCount(); j++) {
|
|
auto profile = profiles->getProfile(j);
|
|
stream_index_pair sip{profile->getType(), 0};
|
|
if (sensors_.find(sip) != sensors_.end()) {
|
|
continue;
|
|
}
|
|
sensors_[sip] = sensor;
|
|
}
|
|
}
|
|
for (const auto &[stream_index, enable] : enable_stream_) {
|
|
if (enable && sensors_.find(stream_index) == sensors_.end()) {
|
|
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_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
|
|
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
|
|
}
|
|
if (!echo_mode_.empty() &&
|
|
device_->isPropertySupported(OB_PROP_LIDAR_SPECIFIC_MODE_INT, OB_PERMISSION_READ_WRITE)) {
|
|
if (echo_mode_ == "Last Echo") {
|
|
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 0);
|
|
} else if (echo_mode_ == "First Echo") {
|
|
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
|
|
}
|
|
RCLCPP_INFO_STREAM(
|
|
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,
|
|
OB_PERMISSION_READ_WRITE)) {
|
|
auto range = device_->getIntPropertyRange(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT);
|
|
if (repetitive_scan_mode_ < range.min || repetitive_scan_mode_ > range.max) {
|
|
RCLCPP_ERROR(logger_,
|
|
"repetitive scan mode value is out of range[%d,%d], please check the value",
|
|
range.min, range.max);
|
|
} else {
|
|
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
|
|
repetitive_scan_mode_);
|
|
RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
|
|
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
|
|
}
|
|
}
|
|
if (filter_level_ != -1 &&
|
|
device_->isPropertySupported(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
|
|
auto range = device_->getIntPropertyRange(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT);
|
|
if (filter_level_ < range.min || filter_level_ > range.max) {
|
|
RCLCPP_ERROR(logger_, "filter level value is out of range[%d,%d], please check the value",
|
|
range.min, range.max);
|
|
} 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_, "Current filter level: " << device_->getIntProperty(
|
|
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
|
|
}
|
|
}
|
|
|
|
if (vertical_fov_ != -1.0 &&
|
|
device_->isPropertySupported(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, OB_PERMISSION_READ_WRITE)) {
|
|
auto range = device_->getFloatPropertyRange(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT);
|
|
if (vertical_fov_ < range.min || vertical_fov_ > range.max) {
|
|
RCLCPP_ERROR(logger_, "vertical fov value is out of range[%f,%f], please check the value",
|
|
range.min, range.max);
|
|
} else {
|
|
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
|
|
RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
|
|
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
|
|
}
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::setupProfiles() {
|
|
for (const auto &elem : LIDAR_STREAMS) {
|
|
if (enable_stream_[elem]) {
|
|
const auto &sensor = sensors_[elem];
|
|
CHECK_NOTNULL(sensor.get());
|
|
auto profiles = sensor->getStreamProfileList();
|
|
CHECK_NOTNULL(profiles.get());
|
|
CHECK(profiles->getCount() > 0);
|
|
for (size_t i = 0; i < profiles->getCount(); i++) {
|
|
auto base_profile = profiles->getProfile(i)->as<ob::LiDARStreamProfile>();
|
|
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) {
|
|
throw std::runtime_error("Failed cast profile to LiDARStreamProfile");
|
|
}
|
|
RCLCPP_DEBUG_STREAM(logger_,
|
|
"Sensor profile: "
|
|
<< "stream_type: " << magic_enum::enum_name(profile->getType())
|
|
<< "Scan Rate: " << magic_enum::enum_name(profile->getScanRate())
|
|
<< "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_[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) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to get "
|
|
<< stream_name_[elem] << " profile: "
|
|
<< orbbec_camera::formatObErrorWithStatus(ex));
|
|
RCLCPP_ERROR_STREAM(logger_, "Stream: " << magic_enum::enum_name(elem.first)
|
|
<< ", Stream Index: " << elem.second
|
|
<< ", Scan Rate: " << rate_[elem]
|
|
<< "Format:" << format_[elem]);
|
|
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
|
|
printSensorProfiles(sensor);
|
|
RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
|
|
exit(-1);
|
|
}
|
|
if (!selected_profile) {
|
|
RCLCPP_WARN_STREAM(
|
|
logger_, "Requested 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 the default profile instead");
|
|
RCLCPP_WARN_STREAM(
|
|
logger_, "Default profile: scan_rate="
|
|
<< magic_enum::enum_name(default_profile->getScanRate())
|
|
<< ", 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_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
|
|
for (const auto &stream_index : HID_STREAMS) {
|
|
if (!enable_stream_[stream_index]) {
|
|
continue;
|
|
}
|
|
try {
|
|
auto profile_list = sensors_[stream_index]->getStreamProfileList();
|
|
if (stream_index == ACCEL) {
|
|
auto full_scale_range = fullAccelScaleRangeFromString(accel_range_);
|
|
auto sample_rate = sampleRateFromString(imu_rate_);
|
|
auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
|
|
stream_profile_[stream_index] = profile;
|
|
} else if (stream_index == GYRO) {
|
|
auto full_scale_range = fullGyroScaleRangeFromString(gyro_range_);
|
|
auto sample_rate = sampleRateFromString(imu_rate_);
|
|
auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
|
|
stream_profile_[stream_index] = profile;
|
|
}
|
|
RCLCPP_INFO_STREAM(logger_, "Stream " << stream_name_[stream_index] << " full scale range: "
|
|
<< (stream_index == ACCEL ? accel_range_ : gyro_range_)
|
|
<< ", sample rate: " << imu_rate_);
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_INFO_STREAM(logger_, "Failed to set up " << stream_name_[stream_index] << " profile: "
|
|
<< orbbec_camera::formatObErrorWithStatus(e));
|
|
enable_stream_[stream_index] = false;
|
|
stream_profile_[stream_index] = nullptr;
|
|
}
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::selectBaseStream() {
|
|
enable_stream_[LIDAR] = true;
|
|
if (enable_stream_[LIDAR]) {
|
|
base_stream_ = LIDAR;
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
|
|
auto profiles = sensor->getStreamProfileList();
|
|
for (size_t i = 0; i < profiles->getCount(); i++) {
|
|
auto origin_profile = profiles->getProfile(i);
|
|
if (sensor->getType() == OB_SENSOR_LIDAR) {
|
|
auto profile = origin_profile->as<ob::LiDARStreamProfile>();
|
|
RCLCPP_INFO_STREAM(logger_, "lidar scan rate: "
|
|
<< profile->getScanRate()
|
|
<< " format:" << magic_enum::enum_name(profile->getFormat()));
|
|
} else {
|
|
RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
|
|
}
|
|
}
|
|
}
|
|
void OBLidarNode::setupPublishers() {
|
|
auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
|
|
if (use_intra_process_) {
|
|
point_cloud_qos_profile = rmw_qos_profile_default;
|
|
}
|
|
if (!enable_scan_to_point_ && format_[LIDAR] == OB_FORMAT_LIDAR_SCAN) {
|
|
scan_pub_ = node_->create_publisher<sensor_msgs::msg::LaserScan>(
|
|
"scan/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
|
|
point_cloud_qos_profile));
|
|
} else if (enable_scan_to_point_ || format_[LIDAR] == OB_FORMAT_LIDAR_POINT ||
|
|
format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
|
|
point_cloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>(
|
|
"cloud/points", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(point_cloud_qos_profile),
|
|
point_cloud_qos_profile));
|
|
}
|
|
if (enable_imu_) {
|
|
std::string topic_name = "imu/sample";
|
|
auto data_qos = getRMWQosProfileFromString(imu_qos_);
|
|
if (use_intra_process_) {
|
|
data_qos = rmw_qos_profile_default;
|
|
}
|
|
imu_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
|
|
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
|
|
}
|
|
auto extrinsics_qos = rclcpp::QoS(1).transient_local();
|
|
if (use_intra_process_) {
|
|
extrinsics_qos = rclcpp::QoS(1);
|
|
}
|
|
if (enable_imu_) {
|
|
lidar_to_imu_extrinsics_publisher_ =
|
|
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
|
|
"/" + camera_name_ + "/lidar_to_imu", extrinsics_qos);
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::startStreams() {
|
|
if (pipeline_ != nullptr) {
|
|
pipeline_.reset();
|
|
}
|
|
pipeline_ = std::make_unique<ob::Pipeline>(device_);
|
|
|
|
try {
|
|
setupPipelineConfig();
|
|
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
|
|
onNewFrameSetCallback(frame_set);
|
|
});
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_ERROR_STREAM(logger_,
|
|
"Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
|
|
setupPipelineConfig();
|
|
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
|
|
onNewFrameSetCallback(frame_set);
|
|
});
|
|
} catch (...) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
|
|
throw std::runtime_error("Failed to start pipeline");
|
|
}
|
|
pipeline_started_.store(true);
|
|
}
|
|
|
|
void OBLidarNode::startIMU() {
|
|
if (!enable_imu_) {
|
|
return;
|
|
}
|
|
|
|
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(accel_range_);
|
|
auto accel_rate = sampleRateFromString(imu_rate_);
|
|
auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
|
|
|
|
// GYRO
|
|
auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
|
|
auto gyro_range = fullGyroScaleRangeFromString(gyro_range_);
|
|
auto gyro_rate = sampleRateFromString(imu_rate_);
|
|
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);
|
|
TRY_EXECUTE_BLOCK(imuPipeline_->enableFrameSync());
|
|
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);
|
|
if (aFrame && gFrame) {
|
|
onNewIMUFrameCallback(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_, "Started IMU stream with accel range: "
|
|
<< fullAccelScaleRangeToString(accel_range)
|
|
<< ", gyro range: " << fullGyroScaleRangeToString(gyro_range)
|
|
<< ", rate: " << sampleRateToString(accel_rate));
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::stopStreams() {
|
|
if (!pipeline_started_ || !pipeline_) {
|
|
RCLCPP_DEBUG_STREAM(logger_, "Pipeline not started or not exist, skip stop pipeline");
|
|
return;
|
|
}
|
|
try {
|
|
pipeline_->stop();
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_ERROR_STREAM(logger_,
|
|
"Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
|
|
} catch (...) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::stopIMU() {
|
|
if (!enable_imu_) {
|
|
return;
|
|
}
|
|
|
|
if (!imu_sync_output_start_ || !imuPipeline_) {
|
|
RCLCPP_DEBUG_STREAM(logger_,
|
|
"IMU pipeline not started or unavailable, skip stopping IMU pipeline");
|
|
return;
|
|
}
|
|
try {
|
|
imuPipeline_->stop();
|
|
imu_sync_output_start_ = false;
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_ERROR_STREAM(
|
|
logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
|
|
} catch (...) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::setupPipelineConfig() {
|
|
if (pipeline_config_) {
|
|
pipeline_config_.reset();
|
|
}
|
|
pipeline_config_ = std::make_shared<ob::Config>();
|
|
for (const auto &stream_index : LIDAR_STREAMS) {
|
|
if (enable_stream_[stream_index]) {
|
|
RCLCPP_DEBUG_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_DEBUG_STREAM(logger_,
|
|
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
|
|
<< " "
|
|
<< magic_enum::enum_name(video_profile->getFormat()));
|
|
}
|
|
|
|
RCLCPP_INFO_STREAM(
|
|
logger_, "Stream " << stream_name_[stream_index]
|
|
<< " scan rate: " << magic_enum::enum_name(profile->getScanRate())
|
|
<< " format: " << magic_enum::enum_name(profile->getFormat()));
|
|
pipeline_config_->enableStream(stream_profile_[stream_index]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &accelframe,
|
|
const std::shared_ptr<ob::Frame> &gryoframe) {
|
|
if (!is_camera_node_initialized_) {
|
|
return;
|
|
}
|
|
if (!imu_publisher_) {
|
|
RCLCPP_ERROR_STREAM(logger_, "IMU publisher not initialized");
|
|
return;
|
|
}
|
|
|
|
if (!tf_published_) {
|
|
publishStaticTransforms();
|
|
tf_published_ = true;
|
|
}
|
|
|
|
bool has_subscriber = imu_publisher_->get_subscription_count() > 0;
|
|
if (!has_subscriber) {
|
|
return;
|
|
}
|
|
|
|
auto imu_msg = sensor_msgs::msg::Imu();
|
|
setDefaultIMUMessage(imu_msg);
|
|
|
|
imu_msg.header.frame_id = accel_gyro_frame_id_;
|
|
auto frame_timestamp = getFrameTimestampUs(accelframe);
|
|
auto timestamp = fromUsToROSTime(frame_timestamp);
|
|
imu_msg.header.stamp = timestamp;
|
|
|
|
auto gyro_frame = gryoframe->as<ob::GyroFrame>();
|
|
auto gyroData = gyro_frame->getValue();
|
|
|
|
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->getValue();
|
|
imu_msg.linear_acceleration.x = -accelData.x;
|
|
imu_msg.linear_acceleration.y = accelData.y;
|
|
imu_msg.linear_acceleration.z = -accelData.z;
|
|
|
|
imu_publisher_->publish(imu_msg);
|
|
}
|
|
|
|
void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
|
|
if (!is_running_.load()) {
|
|
return;
|
|
}
|
|
if (!is_camera_node_initialized_.load()) {
|
|
return;
|
|
}
|
|
if (frame_set == nullptr) {
|
|
return;
|
|
}
|
|
try {
|
|
RCLCPP_INFO_ONCE(logger_, "New frame received");
|
|
if (!tf_published_ && !enable_imu_) {
|
|
publishStaticTransforms();
|
|
tf_published_ = true;
|
|
}
|
|
|
|
// Handle multi-frame publishing for LIDAR_POINT and LIDAR_SPHERE_POINT formats
|
|
if ((format_[LIDAR] == OB_FORMAT_LIDAR_POINT ||
|
|
format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT)) {
|
|
if (publish_n_pkts_ == 1) {
|
|
if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) {
|
|
publishPointCloud(frame_set);
|
|
return;
|
|
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
|
|
publishSpherePointCloud(frame_set);
|
|
return;
|
|
}
|
|
}
|
|
std::lock_guard<std::mutex> lock(frame_buffer_mutex_);
|
|
frame_buffer_.push_back(frame_set);
|
|
// If we have enough frames, publish merged point cloud
|
|
if (frame_buffer_.size() >= static_cast<size_t>(publish_n_pkts_)) {
|
|
if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) {
|
|
publishMergedPointCloud();
|
|
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
|
|
publishMergedSpherePointCloud();
|
|
}
|
|
frame_buffer_.clear();
|
|
}
|
|
} else {
|
|
// Original single frame publishing logic
|
|
if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && !enable_scan_to_point_) {
|
|
publishScan(frame_set);
|
|
} else if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && enable_scan_to_point_) {
|
|
publishScanToPoint(frame_set);
|
|
}
|
|
}
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_ERROR_STREAM(
|
|
logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
|
|
} catch (const std::exception &e) {
|
|
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
|
|
} catch (...) {
|
|
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::publishScan(std::shared_ptr<ob::FrameSet> frame_set) {
|
|
(void)frame_set;
|
|
if (angle_increment_ == 0.0) {
|
|
angle_increment_ = getScanAngleIncrement(rate_[LIDAR]);
|
|
}
|
|
if (frame_set == nullptr) {
|
|
return;
|
|
}
|
|
// std::shared_ptr<ob::LiDARPointsFrame> lidar_frame;
|
|
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
auto *scans_data = reinterpret_cast<OBLiDARScanPoint *>(lidar_frame->getData());
|
|
auto scan_count = lidar_frame->getDataSize() / sizeof(OBLiDARScanPoint);
|
|
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
|
auto timestamp = fromUsToROSTime(frame_timestamp);
|
|
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
|
|
scan_msg->header.stamp = timestamp;
|
|
scan_msg->header.frame_id = frame_id_[LIDAR];
|
|
scan_msg->angle_min = 0.7853981852531433;
|
|
scan_msg->angle_max = 5.495169162750244;
|
|
scan_msg->angle_increment = angle_increment_;
|
|
scan_msg->time_increment = 1.0 / rate_int_[LIDAR] / scan_count;
|
|
scan_msg->scan_time = 1.0 / rate_int_[LIDAR];
|
|
scan_msg->range_min = min_range_;
|
|
scan_msg->range_max = max_range_;
|
|
scan_msg->ranges.resize(scan_count);
|
|
scan_msg->intensities.resize(scan_count);
|
|
for (size_t i = 0; i < scan_count; ++i) {
|
|
if (scans_data->distance < min_range_ && scans_data->distance > max_range_) {
|
|
scans_data++;
|
|
continue;
|
|
}
|
|
scan_msg->ranges[i] = scans_data[i].distance / 1000.0;
|
|
scan_msg->intensities[i] = scans_data[i].intensity;
|
|
}
|
|
filterScan(*scan_msg);
|
|
scan_pub_->publish(std::move(scan_msg));
|
|
}
|
|
|
|
void OBLidarNode::publishScanToPoint(std::shared_ptr<ob::FrameSet> frame_set) {
|
|
(void)frame_set;
|
|
if (frame_set == nullptr) {
|
|
return;
|
|
}
|
|
if (angle_increment_ == 0.0) {
|
|
angle_increment_ = getScanAngleIncrement(rate_[LIDAR]);
|
|
}
|
|
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
auto *scans_data = reinterpret_cast<OBLiDARScanPoint *>(lidar_frame->getData());
|
|
auto scan_count = lidar_frame->getDataSize() / sizeof(OBLiDARScanPoint);
|
|
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
|
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
|
modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "z", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "intensity", 1,
|
|
sensor_msgs::msg::PointField::UINT8);
|
|
modifier.resize(scan_count);
|
|
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
|
auto timestamp = fromUsToROSTime(frame_timestamp);
|
|
point_cloud_msg->header.stamp = timestamp;
|
|
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
|
point_cloud_msg->height = 1;
|
|
point_cloud_msg->width = scan_count;
|
|
point_cloud_msg->is_dense = true;
|
|
point_cloud_msg->is_bigendian = false;
|
|
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
|
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
|
|
for (size_t i = 0; i < scan_count; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity) {
|
|
double rad = 0.7853981852531433 + angle_increment_ * i;
|
|
*iter_x = static_cast<float>(scans_data[i].distance * cos(rad) / 1000.0);
|
|
*iter_y = static_cast<float>(scans_data[i].distance * sin(rad) / 1000.0);
|
|
*iter_z = static_cast<float>(0.0);
|
|
*iter_intensity = static_cast<uint8_t>(scans_data[i].intensity);
|
|
}
|
|
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
|
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
|
}
|
|
|
|
void OBLidarNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
|
|
(void)frame_set;
|
|
if (frame_set == nullptr) {
|
|
return;
|
|
}
|
|
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
auto *point_data = reinterpret_cast<OBLiDARPoint *>(lidar_frame->getData());
|
|
auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARPoint);
|
|
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
|
auto timestamp = fromUsToROSTime(frame_timestamp);
|
|
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
|
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
|
modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "z", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "intensity", 1,
|
|
sensor_msgs::msg::PointField::UINT8, "tag", 1,
|
|
sensor_msgs::msg::PointField::UINT8);
|
|
modifier.resize(point_count);
|
|
point_cloud_msg->header.stamp = timestamp;
|
|
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
|
point_cloud_msg->height = 1;
|
|
point_cloud_msg->width = point_count;
|
|
point_cloud_msg->is_dense = true;
|
|
point_cloud_msg->is_bigendian = false;
|
|
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
|
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
|
|
for (size_t i = 0; i < point_count;
|
|
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag) {
|
|
*iter_x = static_cast<float>(point_data[i].x / 1000.0);
|
|
*iter_y = static_cast<float>(point_data[i].y / 1000.0);
|
|
*iter_z = static_cast<float>(point_data[i].z / 1000.0);
|
|
*iter_intensity = point_data[i].reflectivity;
|
|
*iter_tag = point_data[i].tag;
|
|
}
|
|
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
|
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
|
}
|
|
|
|
void OBLidarNode::publishSpherePointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
|
|
(void)frame_set;
|
|
if (frame_set == nullptr) {
|
|
return;
|
|
}
|
|
auto lidar_frame = frame_set->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
auto *point_data = reinterpret_cast<OBLiDARSpherePoint *>(lidar_frame->getData());
|
|
auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARSpherePoint);
|
|
auto result_point = spherePointToPoint(point_data, point_count);
|
|
auto frame_timestamp = getFrameTimestampUs(lidar_frame);
|
|
auto timestamp = fromUsToROSTime(frame_timestamp);
|
|
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
|
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
|
modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "z", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "intensity", 1,
|
|
sensor_msgs::msg::PointField::UINT8, "tag", 1,
|
|
sensor_msgs::msg::PointField::UINT8);
|
|
modifier.resize(point_count);
|
|
point_cloud_msg->header.stamp = timestamp;
|
|
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
|
point_cloud_msg->height = 1;
|
|
point_cloud_msg->width = point_count;
|
|
point_cloud_msg->is_dense = true;
|
|
point_cloud_msg->is_bigendian = false;
|
|
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
|
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
|
|
for (size_t i = 0; i < point_count;
|
|
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag) {
|
|
*iter_x = static_cast<float>(result_point[i].x / 1000.0);
|
|
*iter_y = static_cast<float>(result_point[i].y / 1000.0);
|
|
*iter_z = static_cast<float>(result_point[i].z / 1000.0);
|
|
*iter_intensity = result_point[i].reflectivity;
|
|
*iter_tag = result_point[i].tag;
|
|
}
|
|
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
|
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
|
}
|
|
|
|
void OBLidarNode::publishMergedPointCloud() {
|
|
if (frame_buffer_.empty()) {
|
|
return;
|
|
}
|
|
|
|
// Calculate total point count across all frames
|
|
size_t total_point_count = 0;
|
|
std::vector<std::pair<OBLiDARPoint *, size_t>> frame_data;
|
|
std::vector<uint64_t> frame_timestamps;
|
|
|
|
for (const auto &fs : frame_buffer_) {
|
|
auto lidar_frame = fs->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
if (lidar_frame) {
|
|
auto *point_data = reinterpret_cast<OBLiDARPoint *>(lidar_frame->getData());
|
|
auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARPoint);
|
|
frame_data.emplace_back(point_data, point_count);
|
|
frame_timestamps.push_back(getFrameTimestampUs(lidar_frame));
|
|
total_point_count += point_count;
|
|
}
|
|
}
|
|
|
|
if (total_point_count == 0) {
|
|
return;
|
|
}
|
|
|
|
// Create merged point cloud message with offset_time field
|
|
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
|
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
|
modifier.setPointCloud2Fields(
|
|
6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
|
|
"intensity", 1, sensor_msgs::msg::PointField::UINT8, "tag", 1,
|
|
sensor_msgs::msg::PointField::UINT8, "offset_time", 1, sensor_msgs::msg::PointField::UINT32);
|
|
modifier.resize(total_point_count);
|
|
|
|
// Use the timestamp of the latest frame as the header timestamp
|
|
auto timestamp = fromUsToROSTime(frame_timestamps.front());
|
|
point_cloud_msg->header.stamp = timestamp;
|
|
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
|
point_cloud_msg->height = 1;
|
|
point_cloud_msg->width = total_point_count;
|
|
point_cloud_msg->is_dense = true;
|
|
point_cloud_msg->is_bigendian = false;
|
|
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
|
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
|
|
|
// Create iterators
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
|
|
sensor_msgs::PointCloud2Iterator<uint32_t> iter_offset_time(*point_cloud_msg, "offset_time");
|
|
|
|
// Calculate frame time interval based on lidar rate
|
|
double frame_interval_us = 1000000.0 / static_cast<double>(rate_int_[LIDAR]);
|
|
|
|
// Merge all frames with per-point timestamps
|
|
for (size_t frame_idx = 0; frame_idx < frame_data.size(); ++frame_idx) {
|
|
auto [point_data, point_count] = frame_data[frame_idx];
|
|
uint64_t frame_timestamp_us = frame_timestamps[frame_idx];
|
|
|
|
// Calculate time increment per point within this frame (uniform sampling)
|
|
double point_time_increment_us = frame_interval_us / static_cast<double>(point_count);
|
|
|
|
// RCLCPP_INFO_STREAM(logger_, "Frame1 " << frame_idx << ": point_count = " << point_count
|
|
// << ", frame_timestamp_us = " << frame_timestamp_us
|
|
// << ", point_time_increment_us = " <<
|
|
// point_time_increment_us);
|
|
for (size_t i = 0; i < point_count;
|
|
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag, ++iter_offset_time) {
|
|
*iter_x = static_cast<float>(point_data[i].x / 1000.0);
|
|
*iter_y = static_cast<float>(point_data[i].y / 1000.0);
|
|
*iter_z = static_cast<float>(point_data[i].z / 1000.0);
|
|
*iter_intensity = point_data[i].reflectivity;
|
|
*iter_tag = point_data[i].tag;
|
|
|
|
// Calculate per-point offset time in nanoseconds relative to point cloud header timestamp
|
|
double point_timestamp_us =
|
|
static_cast<double>(frame_timestamp_us) + (i * point_time_increment_us);
|
|
uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp
|
|
*iter_offset_time =
|
|
static_cast<uint32_t>((point_timestamp_us - static_cast<double>(header_timestamp_us)) *
|
|
1000.0); // Convert to nanoseconds
|
|
}
|
|
}
|
|
|
|
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
|
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
|
}
|
|
|
|
void OBLidarNode::publishMergedSpherePointCloud() {
|
|
if (frame_buffer_.empty()) {
|
|
return;
|
|
}
|
|
|
|
// Calculate total point count across all frames
|
|
size_t total_point_count = 0;
|
|
std::vector<std::pair<OBLiDARSpherePoint *, size_t>> frame_data;
|
|
std::vector<uint64_t> frame_timestamps;
|
|
|
|
for (const auto &fs : frame_buffer_) {
|
|
auto lidar_frame = fs->getFrame(OB_FRAME_LIDAR_POINTS);
|
|
if (lidar_frame) {
|
|
auto *point_data = reinterpret_cast<OBLiDARSpherePoint *>(lidar_frame->getData());
|
|
auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARSpherePoint);
|
|
frame_data.emplace_back(point_data, point_count);
|
|
frame_timestamps.push_back(getFrameTimestampUs(lidar_frame));
|
|
total_point_count += point_count;
|
|
}
|
|
}
|
|
|
|
if (total_point_count == 0) {
|
|
return;
|
|
}
|
|
|
|
// Create merged point cloud message with timestamp field
|
|
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
|
|
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
|
|
modifier.setPointCloud2Fields(
|
|
6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
|
|
sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
|
|
"intensity", 1, sensor_msgs::msg::PointField::UINT8, "tag", 1,
|
|
sensor_msgs::msg::PointField::UINT8, "offset_time", 1, sensor_msgs::msg::PointField::UINT32);
|
|
modifier.resize(total_point_count);
|
|
|
|
// Use the timestamp of the latest frame as the header timestamp
|
|
auto timestamp = fromUsToROSTime(frame_timestamps.front());
|
|
point_cloud_msg->header.stamp = timestamp;
|
|
point_cloud_msg->header.frame_id = frame_id_[LIDAR];
|
|
point_cloud_msg->height = 1;
|
|
point_cloud_msg->width = total_point_count;
|
|
point_cloud_msg->is_dense = true;
|
|
point_cloud_msg->is_bigendian = false;
|
|
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
|
|
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
|
|
|
|
// Create iterators
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
|
|
sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
|
|
sensor_msgs::PointCloud2Iterator<uint32_t> iter_offset_time(*point_cloud_msg, "offset_time");
|
|
|
|
// Calculate frame time interval based on lidar rate
|
|
double frame_interval_us = 1000000.0 / static_cast<double>(rate_int_[LIDAR]);
|
|
|
|
// Merge all frames with per-point timestamps
|
|
for (size_t frame_idx = 0; frame_idx < frame_data.size(); ++frame_idx) {
|
|
auto [sphere_point_data, point_count] = frame_data[frame_idx];
|
|
uint64_t frame_timestamp_us = frame_timestamps[frame_idx];
|
|
|
|
// Convert sphere points to cartesian points
|
|
auto result_point = spherePointToPoint(sphere_point_data, point_count);
|
|
|
|
// Calculate time increment per point within this frame (uniform sampling)
|
|
double point_time_increment_us = frame_interval_us / static_cast<double>(point_count);
|
|
|
|
// RCLCPP_INFO_STREAM(logger_, "Frame " << frame_idx << ": point_count = " << point_count
|
|
// << ", frame_timestamp_us = " << frame_timestamp_us
|
|
// << ", point_time_increment_us = " <<
|
|
// point_time_increment_us);
|
|
|
|
for (size_t i = 0; i < point_count;
|
|
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag, ++iter_offset_time) {
|
|
*iter_x = static_cast<float>(result_point[i].x / 1000.0);
|
|
*iter_y = static_cast<float>(result_point[i].y / 1000.0);
|
|
*iter_z = static_cast<float>(result_point[i].z / 1000.0);
|
|
*iter_intensity = result_point[i].reflectivity;
|
|
*iter_tag = result_point[i].tag;
|
|
|
|
// Calculate per-point offset time in nanoseconds relative to point cloud header timestamp
|
|
double point_timestamp_us =
|
|
static_cast<double>(frame_timestamp_us) + (i * point_time_increment_us);
|
|
uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp
|
|
*iter_offset_time =
|
|
static_cast<uint32_t>((point_timestamp_us - static_cast<double>(header_timestamp_us)) *
|
|
1000.0); // Convert to nanoseconds
|
|
}
|
|
}
|
|
|
|
*point_cloud_msg = filterPointCloud(*point_cloud_msg);
|
|
point_cloud_pub_->publish(std::move(point_cloud_msg));
|
|
}
|
|
|
|
uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr<ob::Frame> &frame) {
|
|
if (frame == nullptr) {
|
|
RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");
|
|
return 0;
|
|
}
|
|
if (time_domain_ == "device") {
|
|
return frame->getTimeStampUs();
|
|
} else if (time_domain_ == "global") {
|
|
return frame->getGlobalTimeStampUs();
|
|
} else {
|
|
return frame->getSystemTimeStampUs();
|
|
}
|
|
}
|
|
|
|
std::vector<OBLiDARPoint> OBLidarNode::spherePointToPoint(OBLiDARSpherePoint *sphere_point,
|
|
uint32_t point_count) {
|
|
std::vector<OBLiDARPoint> point_cloud;
|
|
point_cloud.resize(point_count);
|
|
OBLiDARPoint *point_data = point_cloud.data();
|
|
for (uint32_t i = 0; i < point_count; ++i) {
|
|
double theta_rad = sphere_point->theta * M_PI / 180.0f; // to unit rad
|
|
double phi_rad = sphere_point->phi * M_PI / 180.0f; // to unit rad
|
|
auto distance = sphere_point->distance;
|
|
auto x = static_cast<float>(distance * cos(theta_rad) * cos(phi_rad));
|
|
auto y = static_cast<float>(distance * sin(theta_rad) * cos(phi_rad));
|
|
auto z = static_cast<float>(distance * sin(phi_rad));
|
|
if (std::isfinite(x) && std::isfinite(y) && std::isfinite(y)) {
|
|
// point: convert to opengl point
|
|
point_data->x = x;
|
|
point_data->y = y;
|
|
point_data->z = z;
|
|
point_data->reflectivity = sphere_point->reflectivity;
|
|
point_data->tag = sphere_point->tag;
|
|
++point_data;
|
|
}
|
|
++sphere_point;
|
|
}
|
|
return point_cloud;
|
|
}
|
|
|
|
void OBLidarNode::filterScan(sensor_msgs::msg::LaserScan &scan) {
|
|
double current_angle = scan.angle_min;
|
|
double max_angle = deg2rad(max_angle_);
|
|
double min_angle = deg2rad(min_angle_);
|
|
// map to 0 - 2 * M_PI
|
|
max_angle = std::fmod(max_angle + M_PI, 2 * M_PI);
|
|
if (max_angle < 0) {
|
|
max_angle += 2 * M_PI;
|
|
}
|
|
|
|
min_angle = std::fmod(min_angle + M_PI, 2 * M_PI);
|
|
if (min_angle < 0) {
|
|
min_angle += 2 * M_PI;
|
|
}
|
|
if (min_angle > max_angle) {
|
|
std::swap(min_angle, max_angle);
|
|
}
|
|
for (size_t i = 0; i < scan.ranges.size(); ++i, current_angle += scan.angle_increment) {
|
|
bool is_angle_in_range = (current_angle >= min_angle && current_angle <= max_angle);
|
|
|
|
bool is_range_in_range = (scan.ranges[i] >= min_range_ && scan.ranges[i] <= max_range_);
|
|
|
|
if (!(is_angle_in_range && is_range_in_range)) {
|
|
scan.ranges[i] = 0;
|
|
scan.intensities[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
sensor_msgs::msg::PointCloud2 OBLidarNode::filterPointCloud(
|
|
sensor_msgs::msg::PointCloud2 &point_cloud) const {
|
|
// Initialize the filtered point cloud
|
|
sensor_msgs::msg::PointCloud2 filtered_point_cloud;
|
|
filtered_point_cloud.header = point_cloud.header;
|
|
filtered_point_cloud.height = point_cloud.height;
|
|
filtered_point_cloud.width = point_cloud.width;
|
|
filtered_point_cloud.is_dense = point_cloud.is_dense;
|
|
filtered_point_cloud.is_bigendian = point_cloud.is_bigendian;
|
|
filtered_point_cloud.fields = point_cloud.fields;
|
|
filtered_point_cloud.point_step = point_cloud.point_step;
|
|
|
|
// Convert filter angles from degrees to radians and normalize to [0, 2π]
|
|
double max_angle = deg2rad(max_angle_);
|
|
double min_angle = deg2rad(min_angle_);
|
|
max_angle = std::fmod(max_angle + M_PI, 2 * M_PI);
|
|
min_angle = std::fmod(min_angle + M_PI, 2 * M_PI);
|
|
if (min_angle < 0) {
|
|
min_angle += 2 * M_PI;
|
|
}
|
|
if (max_angle < 0) {
|
|
max_angle += 2 * M_PI;
|
|
}
|
|
|
|
// Swap angles if min is greater than max
|
|
if (min_angle > max_angle) {
|
|
std::swap(min_angle, max_angle);
|
|
}
|
|
|
|
// Reserve space for filtered point cloud data
|
|
filtered_point_cloud.data.reserve(point_cloud.data.size());
|
|
|
|
// Create iterators for each field
|
|
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud, "x");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud, "y");
|
|
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud, "z");
|
|
|
|
// Process each point
|
|
for (size_t i = 0; i < point_cloud.height * point_cloud.width;
|
|
++i, ++iter_x, ++iter_y, ++iter_z) {
|
|
float x = *iter_x;
|
|
float y = *iter_y;
|
|
float z = *iter_z;
|
|
|
|
// Calculate distance from origin
|
|
float distance = std::sqrt(x * x + y * y + z * z);
|
|
|
|
// Calculate angle and normalize to [0, 2π]
|
|
float angle = std::atan2(y, x);
|
|
angle = std::fmod(angle + 2 * M_PI, 2 * M_PI);
|
|
|
|
// Check if point is within both angle and range limits
|
|
bool is_angle_in_range = (angle >= min_angle && angle <= max_angle);
|
|
bool is_range_in_range = (distance >= min_range_ && distance <= max_range_);
|
|
|
|
if (is_angle_in_range && is_range_in_range) {
|
|
// Keep points within the specified range
|
|
filtered_point_cloud.data.insert(filtered_point_cloud.data.end(),
|
|
point_cloud.data.begin() + i * point_cloud.point_step,
|
|
point_cloud.data.begin() + (i + 1) * point_cloud.point_step);
|
|
} else {
|
|
// Fill zero values for filtered out points
|
|
filtered_point_cloud.data.insert(filtered_point_cloud.data.end(), point_cloud.point_step, 0);
|
|
}
|
|
}
|
|
|
|
// Update row step and resize data
|
|
filtered_point_cloud.row_step = filtered_point_cloud.width * filtered_point_cloud.point_step;
|
|
filtered_point_cloud.data.resize(filtered_point_cloud.height * filtered_point_cloud.row_step);
|
|
|
|
return filtered_point_cloud;
|
|
}
|
|
|
|
void OBLidarNode::publishStaticTransforms() {
|
|
if (!publish_tf_) {
|
|
return;
|
|
}
|
|
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
|
|
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
|
|
calcAndPublishStaticTransform();
|
|
if (tf_publish_rate_ > 0) {
|
|
tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
|
|
} else {
|
|
static_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
|
}
|
|
}
|
|
|
|
void OBLidarNode::calcAndPublishStaticTransform() {
|
|
tf2::Quaternion quaternion_optical, zero_rot;
|
|
zero_rot.setRPY(0.0, 0.0, 0.0);
|
|
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
|
|
tf2::Vector3 zero_trans(0, 0, 0);
|
|
auto base_stream_profile = stream_profile_[base_stream_];
|
|
if (!base_stream_profile) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to get base stream profile");
|
|
return;
|
|
}
|
|
CHECK_NOTNULL(base_stream_profile.get());
|
|
// for (const auto &item : stream_profile_) {
|
|
// auto stream_index = item.first;
|
|
|
|
// auto stream_profile = item.second;
|
|
// if (!stream_profile) {
|
|
// continue;
|
|
// }
|
|
// OBExtrinsic ex;
|
|
// try {
|
|
// ex = stream_profile->getExtrinsicTo(base_stream_profile);
|
|
// } catch (const ob::Error &e) {
|
|
// RCLCPP_ERROR_STREAM(logger_, "Failed to get " << stream_name_[stream_index]
|
|
// << " extrinsic: " << e.getMessage());
|
|
// ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
|
|
// }
|
|
|
|
// auto Q = rotationMatrixToQuaternion(ex.rot);
|
|
// Q = quaternion_optical * Q * quaternion_optical.inverse();
|
|
// tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
|
|
// auto timestamp = node_->now();
|
|
// if (stream_index.first != base_stream_.first) {
|
|
// publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]);
|
|
// }
|
|
// publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
|
|
// optical_frame_id_[stream_index]);
|
|
// RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " <<
|
|
// stream_name_[stream_index]
|
|
// << " to "
|
|
// <<
|
|
// stream_name_[base_stream_]);
|
|
// RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " <<
|
|
// trans[2]); RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", "
|
|
// << Q.getZ()
|
|
// << ", " << Q.getW());
|
|
// }
|
|
if (enable_imu_) {
|
|
static const char *frame_id = "lidar_to_imu_extrinsics";
|
|
OBExtrinsic ex;
|
|
try {
|
|
// Try to get extrinsic from ACCEL first
|
|
ex = base_stream_profile->getExtrinsicTo(stream_profile_[ACCEL]);
|
|
|
|
// Verify if GYRO has the same extrinsic (they should be identical for the same IMU)
|
|
try {
|
|
auto gyro_ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
|
|
// Check if ACCEL and GYRO extrinsics are identical
|
|
bool extrinsics_match = true;
|
|
for (int i = 0; i < 9; i++) {
|
|
if (std::abs(ex.rot[i] - gyro_ex.rot[i]) > 1e-6) {
|
|
extrinsics_match = false;
|
|
break;
|
|
}
|
|
}
|
|
for (int i = 0; i < 3; i++) {
|
|
if (std::abs(ex.trans[i] - gyro_ex.trans[i]) > 1e-6) {
|
|
extrinsics_match = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!extrinsics_match) {
|
|
RCLCPP_WARN_STREAM(logger_, "ACCEL and GYRO have different extrinsics, using ACCEL");
|
|
} else {
|
|
RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical");
|
|
}
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_WARN_STREAM(logger_, "Could not get GYRO extrinsic for verification: "
|
|
<< orbbec_camera::formatObErrorWithStatus(e));
|
|
}
|
|
|
|
} catch (const ob::Error &e) {
|
|
RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: "
|
|
<< orbbec_camera::formatObErrorWithStatus(e));
|
|
try {
|
|
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
|
|
RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
|
|
} catch (const ob::Error &e2) {
|
|
RCLCPP_ERROR_STREAM(logger_, "Failed to get "
|
|
<< frame_id << " extrinsic from both ACCEL and GYRO: "
|
|
<< orbbec_camera::formatObErrorWithStatus(e2));
|
|
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
|
|
}
|
|
}
|
|
lidar_to_imu_extrinsic_ = ex;
|
|
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
|
|
CHECK_NOTNULL(lidar_to_imu_extrinsics_publisher_);
|
|
lidar_to_imu_extrinsics_publisher_->publish(ex_msg);
|
|
|
|
// Publish static TF from lidar to IMU
|
|
auto Q = rotationMatrixToQuaternion(ex.rot);
|
|
Q = quaternion_optical * Q * quaternion_optical.inverse();
|
|
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
|
|
auto timestamp = node_->now();
|
|
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);
|
|
|
|
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());
|
|
}
|
|
}
|
|
|
|
orbbec_camera_msgs::msg::IMUInfo OBLidarNode::createIMUInfo(const stream_index_pair &stream_index) {
|
|
orbbec_camera_msgs::msg::IMUInfo imu_info;
|
|
imu_info.header.frame_id = optical_frame_id_[stream_index];
|
|
imu_info.header.stamp = node_->now();
|
|
|
|
if (stream_index == GYRO) {
|
|
auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
|
|
auto gyro_intrinsics = gyro_profile->getIntrinsic();
|
|
imu_info.noise_density = gyro_intrinsics.noiseDensity;
|
|
imu_info.random_walk = gyro_intrinsics.randomWalk;
|
|
imu_info.reference_temperature = gyro_intrinsics.referenceTemp;
|
|
imu_info.bias = {gyro_intrinsics.bias[0], gyro_intrinsics.bias[1], gyro_intrinsics.bias[2]};
|
|
imu_info.scale_misalignment = {
|
|
gyro_intrinsics.scaleMisalignment[0], gyro_intrinsics.scaleMisalignment[1],
|
|
gyro_intrinsics.scaleMisalignment[2], gyro_intrinsics.scaleMisalignment[3],
|
|
gyro_intrinsics.scaleMisalignment[4], gyro_intrinsics.scaleMisalignment[5],
|
|
gyro_intrinsics.scaleMisalignment[6], gyro_intrinsics.scaleMisalignment[7],
|
|
gyro_intrinsics.scaleMisalignment[8]};
|
|
imu_info.temperature_slope = {
|
|
gyro_intrinsics.tempSlope[0], gyro_intrinsics.tempSlope[1], gyro_intrinsics.tempSlope[2],
|
|
gyro_intrinsics.tempSlope[3], gyro_intrinsics.tempSlope[4], gyro_intrinsics.tempSlope[5],
|
|
gyro_intrinsics.tempSlope[6], gyro_intrinsics.tempSlope[7], gyro_intrinsics.tempSlope[8]};
|
|
} else if (stream_index == ACCEL) {
|
|
auto accel_profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
|
|
auto accel_intrinsics = accel_profile->getIntrinsic();
|
|
imu_info.noise_density = accel_intrinsics.noiseDensity;
|
|
imu_info.random_walk = accel_intrinsics.randomWalk;
|
|
imu_info.reference_temperature = accel_intrinsics.referenceTemp;
|
|
imu_info.bias = {accel_intrinsics.bias[0], accel_intrinsics.bias[1], accel_intrinsics.bias[2]};
|
|
imu_info.gravity = {accel_intrinsics.gravity[0], accel_intrinsics.gravity[1],
|
|
accel_intrinsics.gravity[2]};
|
|
imu_info.scale_misalignment = {
|
|
accel_intrinsics.scaleMisalignment[0], accel_intrinsics.scaleMisalignment[1],
|
|
accel_intrinsics.scaleMisalignment[2], accel_intrinsics.scaleMisalignment[3],
|
|
accel_intrinsics.scaleMisalignment[4], accel_intrinsics.scaleMisalignment[5],
|
|
accel_intrinsics.scaleMisalignment[6], accel_intrinsics.scaleMisalignment[7],
|
|
accel_intrinsics.scaleMisalignment[8]};
|
|
imu_info.temperature_slope = {accel_intrinsics.tempSlope[0], accel_intrinsics.tempSlope[1],
|
|
accel_intrinsics.tempSlope[2], accel_intrinsics.tempSlope[3],
|
|
accel_intrinsics.tempSlope[4], accel_intrinsics.tempSlope[5],
|
|
accel_intrinsics.tempSlope[6], accel_intrinsics.tempSlope[7],
|
|
accel_intrinsics.tempSlope[8]};
|
|
}
|
|
|
|
return imu_info;
|
|
}
|
|
void OBLidarNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
|
|
imu_msg.header.frame_id = "imu_link";
|
|
imu_msg.orientation.x = 0.0;
|
|
imu_msg.orientation.y = 0.0;
|
|
imu_msg.orientation.z = 0.0;
|
|
imu_msg.orientation.w = 1.0;
|
|
|
|
imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
|
imu_msg.linear_acceleration_covariance = {
|
|
liner_accel_cov_, 0.0, 0.0, 0.0, liner_accel_cov_, 0.0, 0.0, 0.0, liner_accel_cov_};
|
|
imu_msg.angular_velocity_covariance = {
|
|
angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_, 0.0, 0.0, 0.0, angular_vel_cov_};
|
|
}
|
|
void OBLidarNode::publishStaticTF(const rclcpp::Time &t, const tf2::Vector3 &trans,
|
|
const tf2::Quaternion &q, const std::string &from,
|
|
const std::string &to) {
|
|
geometry_msgs::msg::TransformStamped msg;
|
|
msg.header.stamp = t;
|
|
msg.header.frame_id = from;
|
|
msg.child_frame_id = to;
|
|
msg.transform.translation.x = trans[2] / 1000.0;
|
|
msg.transform.translation.y = -trans[0] / 1000.0;
|
|
msg.transform.translation.z = -trans[1] / 1000.0;
|
|
msg.transform.rotation.x = q.getX();
|
|
msg.transform.rotation.y = q.getY();
|
|
msg.transform.rotation.z = q.getZ();
|
|
msg.transform.rotation.w = q.getW();
|
|
static_tf_msgs_.push_back(msg);
|
|
}
|
|
|
|
void OBLidarNode::publishDynamicTransforms() {
|
|
RCLCPP_WARN(logger_, "Publishing dynamic camera transforms (/tf) at %g Hz", tf_publish_rate_);
|
|
std::mutex mu;
|
|
std::unique_lock<std::mutex> lock(mu);
|
|
while (rclcpp::ok() && is_running_) {
|
|
tf_cv_.wait_for(lock, std::chrono::milliseconds((int)(1000.0 / tf_publish_rate_)),
|
|
[this] { return (!(is_running_)); });
|
|
{
|
|
rclcpp::Time t = node_->now();
|
|
for (auto &msg : static_tf_msgs_) {
|
|
msg.header.stamp = t;
|
|
}
|
|
dynamic_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace orbbec_lidar
|
|
} // namespace orbbec_camera
|