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

Add accel and gyro to lidar tf

Browse Source
This commit is contained in:
jj
2025-07-11 11:05:06 +08:00
parent f694e03fdb
commit 47cccfc3a5
2 changed files with 55 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_lidar_node.cpp
+52 -1
View File
@@ -439,6 +439,20 @@ void OBLidarNode::setupPublishers() {
// 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_stream_[LIDAR] && enable_stream_[ACCEL]) {
lidar_to_other_extrinsics_publishers_[ACCEL] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/lidar_to_accel", extrinsics_qos);
}
if (enable_stream_[LIDAR] && enable_stream_[GYRO]) {
lidar_to_other_extrinsics_publishers_[GYRO] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/lidar_to_gyro", extrinsics_qos);
}
}
void OBLidarNode::startStreams() {
@@ -1070,7 +1084,14 @@ void OBLidarNode::calcAndPublishStaticTransform() {
if (!stream_profile) {
continue;
}
OBExtrinsic ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
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();
@@ -1088,6 +1109,36 @@ void OBLidarNode::calcAndPublishStaticTransform() {
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
}
if (enable_stream_[LIDAR] && enable_stream_[ACCEL]) {
static const char *frame_id = "lidar_to_accel_extrinsics";
OBExtrinsic ex;
try {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[ACCEL]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
lidar_to_other_extrinsics_[ACCEL] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(lidar_to_other_extrinsics_publishers_[ACCEL]);
lidar_to_other_extrinsics_publishers_[ACCEL]->publish(ex_msg);
}
if (enable_stream_[LIDAR] && enable_stream_[GYRO]) {
static const char *frame_id = "lidar_to_gyro_extrinsics";
OBExtrinsic ex;
try {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_,
"Failed to get " << frame_id << " extrinsic: " << e.getMessage());
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
}
lidar_to_other_extrinsics_[GYRO] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(lidar_to_other_extrinsics_publishers_[GYRO]);
lidar_to_other_extrinsics_publishers_[GYRO]->publish(ex_msg);
}
}
orbbec_camera_msgs::msg::IMUInfo OBLidarNode::createIMUInfo(const stream_index_pair &stream_index) {