i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-11 06:59:49 +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/include/orbbec_camera/ob_lidar_node.h
+3
View File
@@ -226,6 +226,9 @@ class OBLidarNode {
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::LiDARStreamProfile>>> std::map<stream_index_pair, std::vector<std::shared_ptr<ob::LiDARStreamProfile>>>
supported_profiles_; supported_profiles_;
std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_; std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
std::map<stream_index_pair, OBExtrinsic> lidar_to_other_extrinsics_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr>
lidar_to_other_extrinsics_publishers_;
stream_index_pair base_stream_ = LIDAR; stream_index_pair base_stream_ = LIDAR;
std::map<stream_index_pair, bool> enable_stream_; std::map<stream_index_pair, bool> enable_stream_;
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)); // 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() { void OBLidarNode::startStreams() {
@@ -1070,7 +1084,14 @@ void OBLidarNode::calcAndPublishStaticTransform() {
if (!stream_profile) { if (!stream_profile) {
continue; 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); auto Q = rotationMatrixToQuaternion(ex.rot);
Q = quaternion_optical * Q * quaternion_optical.inverse(); 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() RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW()); << ", " << 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) { orbbec_camera_msgs::msg::IMUInfo OBLidarNode::createIMUInfo(const stream_index_pair &stream_index) {