Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

bugfix: gravity vector now rotated to target_frame prior to removal #892

Open
wants to merge 1 commit into
base: ros2
Choose a base branch
from
Open

bugfix: gravity vector now rotated to target_frame prior to removal #892

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 12 additions & 7 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2713,9 +2713,8 @@ bool RosFilter<T>::prepareAcceleration(
state(StateMemberPitch),
state(StateMemberYaw));

// transform state orientation to IMU frame
trans.setBasis(stateTmp * target_frame_trans.getBasis());
rotNorm = trans.getBasis().inverse() * normAcc;
// transform state orientation to target frame
rotNorm = stateTmp.inverse() * normAcc;
} else {
tf2::Quaternion curAttitude;
tf2::fromMsg(msg->orientation, curAttitude);
Expand All @@ -2728,14 +2727,20 @@ bool RosFilter<T>::prepareAcceleration(
trans.setRotation(curAttitude);
if (!relative) {
// curAttitude is the true world-frame attitude of the sensor
rotNorm = trans.getBasis().inverse() * normAcc;
rotNorm = target_frame_trans.getBasis() *
(trans.getBasis().inverse() * normAcc);
} else {
// curAttitude is relative to the initial pose of the sensor.
// Assumption: IMU sensor is rigidly attached to the base_link
// (but a static rotation is possible).
rotNorm = target_frame_trans.getBasis().inverse() * trans.getBasis().inverse() * normAcc;
// Assumption 1: IMU sensor is rigidly attached to the
// target_frame (base_link) (but a static rotation is possible).
// Assumption 2: the initial pose of target_frame (base_link)
// is upright.
rotNorm = target_frame_trans.getBasis() *
(trans.getBasis().inverse() *
(target_frame_trans.getBasis().inverse() * normAcc));
}
}
// Note that acc_tmp and rotNorm are both in target_frame
acc_tmp.setX(acc_tmp.getX() - rotNorm.getX());
acc_tmp.setY(acc_tmp.getY() - rotNorm.getY());
acc_tmp.setZ(acc_tmp.getZ() - rotNorm.getZ());
Expand Down