Change rotate

actions_manager/src/robot.py

@@ -13,23 +13,23 @@ class Robot():
     def __init__(self):
         self.position = Position(0, 0, 0)
 
-    def rotate(self, theta):
+    def rotate_to(self, theta):
 
-        # Optimize the rotation by taking the shortest angle position and inverting rotation direction if needed
         theta = theta % (2 * pi)
 
-        clockwise = False
+        start_theta = self.position.theta
+
+        dTheta = (theta - start_theta) % (2 * pi)
 
-        if theta >= pi:
-            theta = 2 * pi - theta
+        # Optimize the rotation by taking the shortest angle position and inverting rotation direction if needed
+        clockwise = False
+        if dTheta >= pi:
+            theta = (2 * pi - theta) % (2 * pi)
             clockwise = True
         else:  # 0 < theta < pi
             clockwise = False
 
-        start_theta = self.position.theta
-        target_theta = start_theta + theta
-
-        while not compare_theta(target_theta, self.position.theta):
+        while not compare_theta(theta, self.position.theta):
             self.send_vel(
                 0, -ROTATION_SPEED if clockwise else ROTATION_SPEED)
             self.sleep(SLEEP_INTERVAL)

actions_manager/src/tasks/goto.py

@@ -18,15 +18,15 @@ class Goto(Task):
             angle_to_target = compute_signed_angle(
                 self.robot.position, self.target_pos)
             angle_to_target = (angle_to_target + pi) % (2 * pi)
-            self.robot.rotate(angle_to_target)
+            self.robot.rotate_to(angle_to_target)
             self.robot.backward(self.robot.position.distance(self.target_pos))
-            self.robot.rotate(self.target_pos.theta)
+            self.robot.rotate_to(self.target_pos.theta)
         else:
             angle_to_target = compute_signed_angle(
                 self.robot.position, self.target_pos)
-            self.robot.rotate(angle_to_target)
+            self.robot.rotate_to(angle_to_target)
             self.robot.forward(self.robot.position.distance(self.target_pos))
-            self.robot.rotate(self.target_pos.theta)
+            self.robot.rotate_to(self.target_pos.theta)
 
     def is_finished(self):
         """