"euler_from_quaternion": when pitch close to ±90°, roll drift? Any solutions?

edit

I'm working with an IMU to estimate orientation. I provide the raw IMU data to imu_filter_madgwick topic imu/data_raw, it gives me back imu/data which includes a quaternion representing the orientation of the sensor.

Let's say the following code has the quaternion I get from the filter message msg:

quaternion = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)

If I compute euler angles by roll, pitch, yaw = tf.transformations.euler_from_quaternion(quaternion), and print them. I noticed that if the pitch angle is close to ±90°, the roll angle will drift about ±180°. I also noticed that this singularity behavior is mentioned here.

My question is how to solve this problem, and I'm asking because rviz is able to visualize this orientation perfectly without any drifting. So, if I use rviz to visualize the orientation using the same quaternion, rviz doesn't have this problem of roll angle drifts when pitch is close to ±90°. The frame moves smoothly at any roll/pitch/yaw angles.

Below is the code I used for rviz to visualized the same quaternion I used to get euler angles:

quaternion = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
br = tf.TransformBroadcaster()
br.sendTransform((0,0,0), quaternion, rospy.Time.now(), 'imu', 'world')

Then, I use rviz to display the imu frame w.r.t world frame. There's simply no drifting or whatever.