i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 04:29:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

imu_to_tf: fixed fixed_frame to base_frame orientation when IMU's frame z axis is not up accordingly to base frame

Browse Source
This commit is contained in:
matlabbe
2025-03-29 12:36:43 -07:00
parent e1e2ce5ece
commit b0916ad14c
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/nodelets/imu_to_tf.cpp
+4 -2
View File
@@ -87,8 +87,10 @@ private:
} }
tf::StampedTransform tmp; tf::StampedTransform tmp;
tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp); tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp);
tf::Transform t = tmp.inverse()*st*tmp; tf::Quaternion q;
q.setRPY(0.0,0.0,tf::getYaw(tmp.getRotation()));
tf::Transform t = tf::Transform(q)*st*tmp.inverse(); // base_frame orientation
st.setRotation(t.getRotation()); st.setRotation(t.getRotation());
st.child_frame_id_ = baseFrameId_; st.child_frame_id_ = baseFrameId_;
} }