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.

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

Rotation handling for roll/pitch/azimuth #12

Open
prclibo opened this issue Sep 26, 2014 · 1 comment
Open

Rotation handling for roll/pitch/azimuth #12

prclibo opened this issue Sep 26, 2014 · 1 comment

Comments

@prclibo
Copy link

prclibo commented Sep 26, 2014

Hi! Just curious if this repo is still under maintenance?

I noticed that novatel_node.cpp uses tf::createQuaternionMsgFromRollPitchYaw to handle attitude angle roll/pitch/azimuth. tf::createQuaternionMsgFromRollPitchYaw assumes roll is around x and pitch around y. However, according to novatel manuals, in e.g. the PVA log, roll is around y and pitch around x. Azimuth is around z but left-hand rotated. So suggested to change the code to

Eigen::Quaterniond quat(Eigen::AngleAxisd(-ins_pva.azimuth * degrees_to_radians, Eigen::Vector3d::UnitZ()) *
                            Eigen::AngleAxisd(ins_pva.pitch * degrees_to_radians, Eigen::Vector3d::UnitX()) * 
                            Eigen::AngleAxisd(ins_pva.roll * degrees_to_radians, Eigen::Vector3d::UnitY()));
tf::quaternionEigenToMsg(quat, cur_odom_.pose.pose.orientation);
@ozymandium
Copy link
Member

@davidhodo or @chris5108 Could you verify this? We don't have a SPAN here to test with.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants