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

Fix publishing static transform order in OBCameraNode

Browse Source
This commit is contained in:
Joe Dong
2024-07-10 15:38:49 +08:00
parent ddfb1c18d8
commit ee5f7d7253
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+7 -7
View File
@@ -2094,21 +2094,21 @@ void OBCameraNode::calcAndPublishStaticTransform() {
auto Q = rotationMatrixToQuaternion(ex.rot); auto Q = rotationMatrixToQuaternion(ex.rot);
Q = quaternion_optical * Q * quaternion_optical.inverse(); Q = quaternion_optical * Q * quaternion_optical.inverse();
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]); tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " << stream_name_[base_stream_]
<< " to "
<< stream_name_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
auto timestamp = node_->now(); auto timestamp = node_->now();
if (stream_index.first != base_stream_.first) { if (stream_index.first != base_stream_.first) {
if (stream_index.first == OB_STREAM_IR_RIGHT) { if (stream_index.first == OB_STREAM_IR_RIGHT && base_stream_.first == OB_STREAM_DEPTH) {
trans[0] = std::abs(trans[0]); // because left and right ir calibration is error trans[0] = std::abs(trans[0]); // because left and right ir calibration is error
} }
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]); publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], frame_id_[stream_index]);
} }
publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index], publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
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 ((pid == FEMTO_BOLT_PID || pid == FEMTO_MEGA_PID) && enable_stream_[DEPTH] && if ((pid == FEMTO_BOLT_PID || pid == FEMTO_MEGA_PID) && enable_stream_[DEPTH] &&