Revision history [back]

some times you want control yaw(theta) but when you transform quarternion to euler the angles are between pi and -pi and you have one break point in pi and it's bother controller for this reason in this part of code:

if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
else:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)**

if we have jump in angle (this jump is bigger than 2 *pi(-pi to pi)) Compute d_theta and add the d_theta to total_robot_2d_theta