Prevent robot from being stuck in corners

tuto_kickoff/scripts/reactive_move

 #!/usr/bin/python3
 import rclpy
+import math as m
 from rclpy.node import Node
 from sensor_msgs.msg import PointCloud2
 from geometry_msgs.msg import Twist
@@ -8,7 +9,13 @@ import time
 
 class ReactiveMove(Node):
 
-    def __init__(self, name="reactive_move"):
+    MAX_STUCK_TIME = 5 # Time before the robot is considered to be stuck and has to make a U-turn
+    RECT_WIDTH = 0.5
+    RECT_LENGTH = 0.5
+    ANGULAR_VELOCITY = 1.0
+    LINEAR_VELOCITY = 0.2
+    
+    def __init__(self, name="reactive_move", timerFreq = 1/60.0):
         super().__init__(name) # Create the node
 
         # Initialize a subscriber
@@ -17,32 +24,67 @@ class ReactiveMove(Node):
         # Initialize a publisher
         self._velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
 
-        # Clock for the publisher
-        self.create_timer(2, self.publish_velo)
+        # Initialize a clock for the publisher
+        self.create_timer(timerFreq, self.publish_velo)
+
+        # Initialize variables
+        self._timerFreq = timerFreq
+        self._previousScanTime = None
+        self._previousStraightMvtTime = None
+        self._points = []
 
 
 
     def scan_callback(self, scanMsg):
         """
-        Save calculated velocity in _velocity
+        Save received points for future calculations
         """
-        points = sensor_msgs_py.point_cloud2.read_points(scanMsg)
+        self._points = sensor_msgs_py.point_cloud2.read_points(scanMsg)
+
+        self._previousScanTime = time.time()
 
-        x, y = firstPointInFront(points, 0.5, 0.25)
     
-        velocity = Twist()
+    def calcul_velo(self):
+        """
+        Calculate the velocity based on the "map" points and other data
+        """
+
+        now = time.time()
+
+
+        # If scan data received more than 1s ago, stop the robot
+        if now - self._previousScanTime > 1: 
+            print("STOP ROBOT")
+            return Twist()  # Return a zero velocity
+        
+        # If it's been rotating for more than 5s, it's probably stuck in a corner, so we rotate it completely, until >3.14s which is equivalent to a rotation of >180°
+        if self._previousStraightMvtTime and self.MAX_STUCK_TIME < now - self._previousStraightMvtTime <= self.MAX_STUCK_TIME + m.pi: 
+            velo = Twist()
+            velo.angular.z = self.ANGULAR_VELOCITY
+            print("U-TURN")
+            return velo
 
+        # Default case
+        x, y = firstPointInFront(self._points, self.RECT_LENGTH, self.RECT_WIDTH / 2.0)
+        velocity = Twist()
         if x and y:
             if y >= 0:  # Point is in the left part of the rectangle -> Turn right
-                velocity.angular.z = -1.0  
+                velocity.angular.z = -self.ANGULAR_VELOCITY
+                print("TURN RIGHT")
             else:  # Point is in the right part of the rectangle -> Turn left
-                velocity.angular.z = 1.0
+                velocity.angular.z = self.ANGULAR_VELOCITY
+                print("TURN LEFT")
             
         else:  # If there's not point in front, go straight
-            velocity.linear.x = 0.2 
+            velocity.linear.x = self.LINEAR_VELOCITY
+            self._previousStraightMvtTime = now
+            print("STRAIGHT")
+
+        return velocity
+
+
+
 
-        self._velocity = velocity
-        self._previousScanTime = time.time()
 
 
     def publish_velo(self):
@@ -50,13 +92,17 @@ class ReactiveMove(Node):
         Publish velocity independently of the callback
         """
 
-        # Only move if we received scan data since less than a second
-        if(time.time() - self._previousScanTime < 1):  
-            self._velocity_publisher.publish(self._velocity) # Move according to previously calculated velocity
-        else:
-            self._velocity_publisher.publish(Twist()) # Stop movement
+        # Make sure at least one callback is made
+        if not self._previousScanTime:
+            return -1
 
 
+        velocity = self.calcul_velo()
+
+        self._velocity_publisher.publish(velocity) # Move according to previously calculated velocity
+
+        return 0
+
 
 
 def firstPointInFront(points:list, maxX, maxY):
@@ -72,6 +118,9 @@ def firstPointInFront(points:list, maxX, maxY):
 
 
 def main():
+    """
+    Main loop
+    """
     rclpy.init()
     rosNode = ReactiveMove()
     rclpy.spin(rosNode)