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

Merge branch 'master' of github.com:introlab/rtabmap_ros into ros2

Browse Source
This commit is contained in:
matlabbe
2025-03-29 12:54:50 -07:00
parent 3ed09452f6 b0916ad14c
commit a1e204afdb
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/nodelets/imu_to_tf.cpp
+5 -2
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_conversions/MsgConversion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/utils.hpp>
namespace rtabmap_util
{
@@ -81,10 +82,12 @@ void ImuToTF::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
return;
}
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp);
geometry_msgs::msg::TransformStamped tmp = tfBuffer_->lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp);
tf2::Transform tmp_t;
tf2::fromMsg(tmp.transform, tmp_t);
tf2::Transform t = tmp_t.inverse()*st*tmp_t;
tf2::Quaternion q;
q.setRPY(0.0,0.0,tf2::getYaw(tmp_t.getRotation()));
tf2::Transform t = tf2::Transform(q)*st*tmp_t.inverse(); // base_frame orientation
st.setRotation(t.getRotation());
childFrameId = baseFrameId_;
}