Increase angle from first to second quadrant, and then decrease it from third to fourth

Question

I'm developing a robotics application, and I have to rotate the robot to a certain goal. I do it adding some amount to a radian angle.

My problem is that the robot returns its heading in a particular way: it goes from 0 to $\pi/2$, from first quadrant to second quadrant. But when it rotates from second quadrant to third quadrant its heading turns into negative.

This is an example from the headings returned by the robot:

 3.056
 3.11045
 3.13962
-3.10837
-3.04676
-3.00716

This angle will continue decreasing until zero when robots turn from third quadrant to fourth quadrant. And on first quadrant its heading turns into positive again:

 -0.0338163
 -0.0340248
 -0.00649877
  0.044058
  0.109558
  0.159966

My problem is that I'm only add positive values. For example to go from second quadrant to third quadrant I add $\pi/2$ to get the goal angle, and then the robot starts to rotate to get that angle:

goal: 3.13863, theta_: 2.97239
goal: 3.13863, theta_: 3.01199
goal: 3.13863, theta_: 3.056
Turn Left
goal: 4.68124, theta_: 3.11045
goal: 4.68124, theta_: 3.11045
goal: 4.68124, theta_: 3.13962
goal: 4.68124, theta_: -3.10837
goal: 4.68124, theta_: -3.04676
goal: 4.68124, theta_: -3.00716
goal: 4.68124, theta_: -2.95877
goal: 4.68124, theta_: -2.92357

goal is the goal angle, and theta_ is robot's heading.

To compute theta_ I use something called: "Compute Euler from Quaternion". There is a function in ROS (a robotic framework), that will return roll, pitch and yaw (theta_ is yaw) using robot orientation.

This is the Python code to compute it:

def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

Maybe this is a property or something in mathematics that I don't know and I thought it would be a good idea to ask it here.

In a nutshell, I don't know how to pass from $\pi$ to $-\pi$ from second quadrant to third quadrant, and then continue decreasing the angle until 0 on fourth quadrant.

How can I do that?

Answer 1

The value that I'm looking for, $x$, it could be computed this way:

$$x=-\pi + (goal - \pi)$$

What do you think?