Extracting correct yaw angle from quaternion with small roll and pitch angle

edit

Hi,

I am using tf::getYaw() function to extract yaw angle from quaternion. My robot usually moves in 2D with small (although not zero) roll and pitch angles. When I use getYaw() the output jumps back and forth about 180 degree in certain times.

For example

1) r: 3.14137 p: -3.13828 y: 3.13949 2) r: 0.000697 p: -0.004024 y: 0.001175

These 2 RPY values represent almost the same angle, but R,P,Y values are differed by almost 180 degree.

Is there a way to ensure conversion to yaw angle will always be continuous value? My vehicle moves mostly in flat surface with small roll and pitch angles always.

I tried to zero out x,y values of corresponding quaternion but it didn't work.