i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +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
+8 -8
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] &&
@@ -2176,7 +2176,7 @@ void OBCameraNode::calcAndPublishStaticTransform() {
"Failed to get " << frame_id << " extrinsic: " << e.getMessage()); "Failed to get " << frame_id << " extrinsic: " << e.getMessage());
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}}); ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
} }
ex.trans[0] = -std::abs( ex.trans[0]); ex.trans[0] = -std::abs(ex.trans[0]);
depth_to_other_extrinsics_[INFRA2] = ex; depth_to_other_extrinsics_[INFRA2] = ex;
auto ex_msg = obExtrinsicsToMsg(ex, frame_id); auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg); depth_to_other_extrinsics_publishers_[INFRA2]->publish(ex_msg);