fonctionne pas

src/rove_navigation/rove_navigation/lost_connection.py

@@ -1,4 +1,6 @@
+from asyncio import Future
 import socket
+from threading import Thread
 import rclpy
 from rclpy.node import Node
 from rclpy.action import ActionClient
@@ -13,62 +15,63 @@
 class LostNetworkFallback(Node):
 
     def __init__(self):        
-        self.DISTANCE_BETWEEN_FALLBACKS = 0.1 # meters
+        self.DISTANCE_BETWEEN_FALLBACKS = 0.4 # meters
         self.NB_MAX_FAILED_CONNS = 3
-        self.nbFailedConns = 0
-        self.fallbackIndex = 0
         self.PING_RATE = 1 # seconds
 
         self.ONLINE = 'ONLINE'
         self.OFFLINE = 'OFFLINE'
-        self.WAITING = 'WAITING'
 
-        self.state = "ONLINE"
+        self.state = self.ONLINE
         self.traveling_to_fallback = False
-
-        self.goal_handle = None
-
+        self.nbFailedConns = 0
+        self.fallbackIndex = 0
+                
         super().__init__('lost_network_fallback')
         self.get_logger().info('Initiating LostNetworkFallback node')
-        self.create_timer(self.PING_RATE, self.check_connection)
+        self.create_timer(self.PING_RATE, self.fallback_behavior)
         self.odometry_subscription = self.create_subscription(
             Odometry, '/odometry/local', self.odometry_callback, 10)
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
         #list of potential fallback positions, ascending order
         self.fallback_positions = [Point(x=0.0,y=0.0,z=0.0)]
+        self.current_position = Point(x=0.0,y=0.0,z=0.0)
+
         self.fallback_positions.pop()
         self.current_position: Point
+        # Thread to execute goal pose posting (because send_goal_async()'s callback was called before reaching actual goal pose)
+        self.connection_thread = Thread(target=self.check_connection)
+        self.connection_thread.start()
 
+    # gets called on a ros timer (main thread)
+    async def fallback_behavior(self):
+        if self.state==self.OFFLINE and self.fallbackIndex < len(self.fallback_positions):
+            await self.navigate_to_fallback()
 
     # Check connection to google.com TODO: check connection to UI user instead
+    # Gets called on a different thread
     def check_connection(self):
-        try:
-            socket.setdefaulttimeout(1) # in seconds
-            socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect(("8.8.8.8", 53))
-            # if this following code runs, the connection was established
-            self.nbFailedConns = 0
-            if self.state != self.ONLINE:
-                self.get_logger().info('Connection re-established to server! terminating fallback state')
-                self.fallback_positions[self.fallbackIndex:]
-                if self.goal_handle != None:
-                    self.action_client._cancel_goal_async(self.goal_handle)
-                    self.goal_handle = None
-            self.state = self.ONLINE
-        except socket.error:
-            self.nbFailedConns += 1
-            if self.state != self.WAITING and self.nbFailedConns >= self.NB_MAX_FAILED_CONNS:
-                self.state = self.OFFLINE
-                if self.nbFailedConns == self.NB_MAX_FAILED_CONNS: 
-                    self.get_logger().warn(f'Connection Error!!! ping to server failed {self.nbFailedConns} '+
-                                       'times in a row, activating fallback state, the robot will try to retrace its steps.')
-                # TODO: cancel current state of general state_handler
-                if not self.traveling_to_fallback and self.fallbackIndex < len(self.fallback_positions):
-                    self.traveling_to_fallback = True
-                    self.navigate_to_fallback()
-
+        while True:
+            try:
+                socket.setdefaulttimeout(1) # in seconds
+                socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect(("8.8.8.8", 53))
+                # if this following code runs, the connection was established
+                self.nbFailedConns = 0
+                if self.state != self.ONLINE:
+                    self.get_logger().info('Connection re-established to server! terminating fallback state')
+                    self.fallback_positions[self.fallbackIndex:]
+                    self.fallbackIndex = 0
+                self.state = self.ONLINE
+            except socket.error:
+                self.nbFailedConns += 1
+                if self.nbFailedConns >= self.NB_MAX_FAILED_CONNS:
+                    self.state = self.OFFLINE
+                    if self.nbFailedConns == self.NB_MAX_FAILED_CONNS: 
+                        self.get_logger().warn(f'Connection Error!!! ping to server failed {self.nbFailedConns} '+
+                                           'times in a row, activating fallback state, the robot will try to retrace its steps.')                    
 
-    def navigate_to_fallback(self):
+    async def navigate_to_fallback(self):
         # Prepare the goal message and send it
         goal_pose = PoseStamped()
         goal_pose.header = Header(frame_id='map')
@@ -81,39 +84,52 @@ def navigate_to_fallback(self):
 
         goal_msg = NavigateToPose.Goal()
         goal_msg.pose = goal_pose
-        future = self.action_client.send_goal_async(goal_msg)
-        self.goal_handle = future.result()
-        future.add_done_callback(self.fallback_point_reached)
-
-
-    def fallback_point_reached(self, _):        
-        self.traveling_to_fallback = False            
-        if self.state == self.OFFLINE and self.fallbackIndex < len(self.fallback_positions):
+        goal_handle = await self.action_client.send_goal(goal_msg) ## the future returned from this does NOT WORK >:(
+        while not self.is_robot_close_to_goal():
+            if self.state == self.ONLINE: 
+                # TODO: cancel current state of general state_handler, right now only cancels return to fallback
+                self.action_client._cancel_goal_async(goal_handle) 
+                return # end fallback loop
+
+        self.fallbackIndex+=1
+        if self.fallbackIndex < len(self.fallback_positions):
             self.get_logger().warn(f'Fallback {self.fallbackIndex} reached. Will attempt to reach next one if still offline.')
-            self.fallbackIndex+=1
-        if self.state == self.OFFLINE and self.fallbackIndex >= len(self.fallback_positions):
-            self.state = self.WAITING
+
+        if self.fallbackIndex == len(self.fallback_positions):
             self.get_logger().error(f'Last fallback reached, waiting and hoping for connection to come back :(')
 
+        self.traveling_to_fallback = False
+
+
+    def is_robot_close_to_goal(self):
+        fall = self.fallback_positions[self.fallbackIndex]
+        dist = [self.current_position.x-fall.x,
+                self.current_position.y-fall.y,
+                self.current_position.z-fall.z]
+        self.get_logger().info('fallback_positions[i]: '+str(fall))
+        self.get_logger().info('self.current_position: '+str(self.current_position))
+        self.get_logger().info('distance: '+str(np.linalg.norm(dist)))
+        return np.linalg.norm(dist) <= 0.3
+
     def odometry_callback(self, msg : Odometry):
-        current_position = Point(x=msg.pose.pose.position.x, y=msg.pose.pose.position.y, z=msg.pose.pose.position.z)
+        self.current_position = Point(x=msg.pose.pose.position.x, y=msg.pose.pose.position.y, z=msg.pose.pose.position.z)
         # Sets the first position
         if len(self.fallback_positions) == 0: 
-            self.fallback_positions = [current_position] 
+            self.fallback_positions = [self.current_position] 
             self.get_logger().info(f'First fallback position initiated: {self.fallback_positions[0]}')            
 
         # If robot at self.DISTANCE_BETWEEN_FALLBACKS distance or more from last fallback, add new fallback point 
         if(self.state == self.ONLINE):
             if len(self.fallback_positions) == 0: 
-                self.fallback_positions.append(current_position)
+                self.fallback_positions.append(self.current_position)
             else:
-                dist = [current_position.x-self.fallback_positions[0]._x,
-                        current_position.y-self.fallback_positions[0]._y,
-                        current_position.z-self.fallback_positions[0]._z]
+                dist = [self.current_position.x - self.fallback_positions[0].x,
+                        self.current_position.y - self.fallback_positions[0].y,
+                        self.current_position.z - self.fallback_positions[0].z]
 
                 if(np.linalg.norm(dist) >= self.DISTANCE_BETWEEN_FALLBACKS):
-                    self.fallback_positions.insert(0, current_position)
-                    self.get_logger().info(f'New fallback position added: {self.fallback_positions}')            
+                    self.fallback_positions.insert(0, self.current_position)
+                    self.get_logger().info(f'New fallback position added: {self.fallback_positions[0].__str__()} total fallback points: {len(self.fallback_positions)}')            
 
 def main(args=None):
     rclpy.init(args=args)