Visualize object orientation using IMU
Hello my question might sound a little naive I know. But, you know how tedious and confusing quaternions might be.
I have an OAK-D camera with it's model in stl format have a URDF provided with all the links and joints for everything but mainly the base_link of the camera which is been sat arbitrarily around the middle camera center and the offset xyz and orientation rpy of the imu_link relative to the base_link.
P.S. I use depthai to bridge the readings with ROS and use magdwick filter for IMU raw data.
Now I want the camera model to move (orientation only) as I have the orientation data provided by an IMU filter which is visualized just fine and accurate. I want to publish a transform from the footprint to base_link with the orientation published by the IMU but there's always a misalignment between the rotations axes. I've looked it up but haven't found something specific regarding this issue. Eventually I made a node that uses relative rotations with quaternions and they aligned now they follow one another but in reality the z (blue) axis is forward instead of upwards you can see in the image camera is pointing to the ceiling while the z-axis is forward (body upright). here's an image https://imgur.com/a/mWwMPh1
so in my mind a simple solution was to rotate the quat. around y-axis and I wrote this code
orientation_q = msg.orientation
q_imu = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
q_rot = quaternion_from_euler(0, -1.5708, 0)
q_imu_rot = quaternion_multiply(q_rot, q_imu)
# Origin quat
q1_inv = [0, 0, 0, -1]
# Relative rot quat from origin to q_imu_rot
q_new = quaternion_multiply(q_imu_rot, q1_inv)
Then I send this transform. But it leads to other misalignment. I simply want all the resulting orientations to be rotated -90 degrees so my model follows my real camera orientation. Probably there's a smarter solution ik :D Thanks in advance.
