Equivalent of checking quaternion is valid in tf2

edit

There is a thread that may be interesting to you:

https://github.com/ros/geometry2/pull...

I'm having trouble understanding this idea that a calculation should be refused because the quaternion is slightly un-normalized. It seems reasonable to log a warning if you think that characteristic is important, but by my thinking you should proceed with the calculation anyway.

Re-implementing that source code in python:

q = transform_stamped.transform.rotation
q_norm_error = abs(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1.0)
text = f"quaternion norm error {q_norm_error:0.6f}, {q}".replace("\n", ", ")
if q_norm_error > 0.01:
    ...

Probably converting the quaternion to the transformations.py array representation and doing a dot with itself is better