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

imu_to_tf: fixed tf orientation if there is a rotation between base frame and imu frame. OdometryROS: fixed rotation imu init when there is a rotation between imu and base frame

Browse Source
This commit is contained in:
matlabbe
2020-02-25 10:55:28 -05:00
parent f54e060787
commit 3207b412ce
2 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+1 -1
View File
@@ -461,7 +461,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
if(imu.orientation()[0] != 0 || imu.orientation()[1] != 0 || imu.orientation()[2] != 0 || imu.orientation()[3] != 0) if(imu.orientation()[0] != 0 || imu.orientation()[1] != 0 || imu.orientation()[2] != 0 || imu.orientation()[3] != 0)
{ {
Transform rotation(0,0,0, imu.orientation()[0], imu.orientation()[1], imu.orientation()[2], imu.orientation()[3]); Transform rotation(0,0,0, imu.orientation()[0], imu.orientation()[1], imu.orientation()[2], imu.orientation()[3]);
rotation = rotation * imu.localTransform().rotation().inverse(); rotation = imu.localTransform().rotation() * rotation * imu.localTransform().rotation().inverse();
this->reset(rotation); this->reset(rotation);
float r,p,y; float r,p,y;
rotation.getEulerAngles(r,p,y); rotation.getEulerAngles(r,p,y);
src/nodelets/imu_to_tf.cpp
+2 -1
View File
@@ -88,7 +88,8 @@ private:
tf::StampedTransform tmp; tf::StampedTransform tmp;
tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp); tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
st *= tmp; tf::Transform t = tmp.inverse()*st*tmp;
st.setRotation(t.getRotation());
st.child_frame_id_ = baseFrameId_; st.child_frame_id_ = baseFrameId_;
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)