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

Use imu to estimate rotation #527

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Use imu to estimate rotation #527

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
9 changes: 3 additions & 6 deletions bitbots_navigation/bitbots_odometry/src/odometry_fuser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,9 +106,8 @@ class OdometryFuser : public rclcpp::Node {
tf2::Transform base_link_in_rotation_point = rotation_point_in_base.inverse();

// get only translation and yaw from motion odometry
tf2::Quaternion odom_orientation_yaw = getCurrentMotionOdomYaw(motion_odometry.getRotation());
tf2::Transform motion_odometry_yaw;
motion_odometry_yaw.setRotation(odom_orientation_yaw);
motion_odometry_yaw.setRotation(tf2::Quaternion::getIdentity());
motion_odometry_yaw.setOrigin(motion_odometry.getOrigin());

// Get the rotation offset between the IMU and the baselink
Expand All @@ -122,16 +121,14 @@ class OdometryFuser : public rclcpp::Node {
}

// get imu transform without yaw
tf2::Quaternion imu_orientation_without_yaw_component =
getCurrentImuRotationWithoutYaw(imu_orientation * imu_mounting_offset.getRotation());
tf2::Quaternion imu_orientation_without_yaw_component = imu_orientation * imu_mounting_offset.getRotation();
tf2::Transform imu_without_yaw_component;
imu_without_yaw_component.setRotation(imu_orientation_without_yaw_component);
imu_without_yaw_component.setOrigin({0, 0, 0});

// transformation chain to get correctly rotated odom frame
// go to the rotation point in the odom frame. rotate the transform to the base link at this point
fused_odometry =
motion_odometry_yaw * rotation_point_in_base * imu_without_yaw_component * base_link_in_rotation_point;
fused_odometry = motion_odometry_yaw * imu_without_yaw_component;
} else {
fused_odometry = motion_odometry;
}
Expand Down
Loading