Add reactive_move script that moves and avoids obstacles

tuto_kickoff/CMakeLists.txt

@@ -36,5 +36,6 @@ install( PROGRAMS
   scripts/myNode
   scripts/tuto_move
   scripts/tuto_scan_echo
+  scripts/reactive_move
   DESTINATION lib/${PROJECT_NAME}
 )
\ No newline at end of file

tuto_kickoff/scripts/reactive_move

+#!/usr/bin/python3
+import rclpy
+import math
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import Twist
+import sensor_msgs_py.point_cloud2
+from std_msgs.msg import Header
+
+rosNode= None
+velocity_publisher = None
+
+def scan_callback(scanMsg):
+    global rosNode
+    global velocity_publisher
+    points = sensor_msgs_py.point_cloud2.read_points(scanMsg)
+
+    x, y = firstPointInFront(points, 0.5, 0.25)
+
+    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  
+        else:  # Point is in the right part of the rectangle -> Turn left
+            velocity.angular.z = 1.0
+    
+    else:  # If there's not point in front, go straight
+        velocity.linear.x = 0.2 
+
+    velocity_publisher.publish(velocity)    
+    
+    
+
+def firstPointInFront(points:list, maxX, maxY):
+    for (x,y,_) in points:
+        if(x>0 and x < maxX and abs(y) < maxY):
+            return (x,y)
+    
+    return (None, None)
+
+
+
+def main():
+    global velocity_publisher
+
+    rclpy.init()
+    rosNode= Node('reactive_move')
+    # Initialize a subscriber:
+    rosNode.create_subscription( PointCloud2, 'scan_points', scan_callback, 10)
+    # Initialize a publisher:
+    velocity_publisher = rosNode.create_publisher(Twist, '/cmd_vel', 10)
+
+    while True :
+        rclpy.spin_once( rosNode )
+
+
+    scanInterpret.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file