Arrêt du robot quand les roues ne touchent pas le sol

grp_astro/src/reactive_move

@@ -6,6 +6,7 @@ import rclpy
 import math as m
 from rclpy.node import Node
 from sensor_msgs.msg import PointCloud2
+from kobuki_ros_interfaces.msg import WheelDropEvent
 from geometry_msgs.msg import Twist
 import sensor_msgs_py.point_cloud2
 import time
@@ -30,8 +31,9 @@ class ReactiveMove(Node):
         """ constructor """
         super().__init__(name)  # Create the node
 
-        # Initialize a subscriber
-        self.create_subscription(PointCloud2, 'scan_points', self.scan_callback, 10)
+        # Initialize subscribers
+        self.create_subscription(PointCloud2, '/scan_points', self.scan_callback, 10)
+        self.create_subscription(WheelDropEvent, '/events/wheel_drop', self.wheel_callback, 10)
 
         # Initialize a publisher
         self._velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
@@ -44,6 +46,8 @@ class ReactiveMove(Node):
         self._previousScanTime = None
         self._points = []
         self._firstPointSeen = None
+        self._leftWheelDropped = False
+        self._rightWheelDropped = False
 
 
 
@@ -55,6 +59,15 @@ class ReactiveMove(Node):
 
         self._previousScanTime = time.time()
 
+    def wheel_callback(self, msg):
+        """
+        Save received wheel state informations (dropped)
+        """
+        if(msg.wheel == msg.LEFT):
+            self._leftWheelDropped = msg.state == msg.DROPPED
+        else:
+            self._rightWheelDropped = msg.state == msg.DROPPED
+
     
     def calcul_velo(self):
         """
@@ -69,6 +82,12 @@ class ReactiveMove(Node):
             print("No data received for more than 1s")
             return Twist()  # Return a zero velocity
         
+        # If both robot wheels are dropped (i.e. the robot is not on the floor), stop the robot movement
+        if self._leftWheelDropped and self._rightWheelDropped:
+            print("The robot is not on the floor")
+            return Twist()  # Return a zero velocity
+        
+
         # Default case
         x, y = firstPointInFront(self._points, self.RECT_LENGTH, self.RECT_WIDTH / 2.0)