added lost_connection behavior (pain and suffering)

Author: nathan-gt

src/rove_navigation/rove_navigation/lost_connection.py

@@ -2,87 +2,118 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.action import ActionClient
-from sensor_msgs.msg import PointCloud2
-import sensor_msgs_py.point_cloud2 as pc2
+from geometry_msgs.msg import PoseArray
 from geometry_msgs.msg import Point, PoseStamped
 from nav2_msgs.action import NavigateToPose
 import numpy as np
 from std_msgs.msg import Header
 from nav_msgs.msg import Odometry
-import constants
 
 # Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
 class LostNetworkFallback(Node):
 
-    def __init__(self):
-        self.NB_FALLBACKS=100
-        self.DISTANCE_BETWEEN_FALLBACKS = 2 # meters
+    def __init__(self):        
+        self.DISTANCE_BETWEEN_FALLBACKS = 0.1 # 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.traveling_to_fallback = False
 
-        self.state = constants.OFFLINE
         self.goal_handle = None
 
         super().__init__('lost_network_fallback')
+        self.get_logger().info('Initiating LostNetworkFallback node')
         self.create_timer(self.PING_RATE, self.check_connection)
         self.odometry_subscription = self.create_subscription(
             Odometry, '/odometry/local', self.odometry_callback, 10)
-        self.action_client = ActionClient(self, LostNetworkFallback, 'lost_network_fallback')
+        self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
-        #list of potential fallback positions, always will be of size self.NB_FALLBACKS, ascending order
-        self.fallback_position = [Point(x=0.0, y=0.0, z=0.0) for _ in range(self.NB_FALLBACKS)]
-        self.current_position = Point(x=0.0, y=0.0, z=0.0)
+        #list of potential fallback positions, ascending order
+        self.fallback_positions = [Point(x=0.0,y=0.0,z=0.0)]
+        self.fallback_positions.pop()
+        self.current_position: Point
+    
 
     # Check connection to google.com TODO: check connection to UI user instead
     def check_connection(self):
         try:
             socket.setdefaulttimeout(1) # in seconds
             socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect(("8.8.8.8", 53))
-            self.nbFailedConns = -1
-            if self.goal_handle != None:
-                self.action_client._cancel_goal_async(self.goal_handle)
-                self.goal_handle = None
-        except:
+            # 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.nbFailedConns == self.NB_MAX_FAILED_CONNS:
-                # TODO: cancel current state
-                self.navigate_to_fallback(self.fallbackIndex)
-    
-    def odometry_callback(self, msg : Odometry):
-        # If robot at self.DISTANCE_BETWEEN_FALLBACKS distance or more from last fallback, add new fallback, delete the last one from the list 
-        self.current_position = Point(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z, )
-        if(np.linalg.norm(self.current_position-self.fallback_position[0]) >= self.DISTANCE_BETWEEN_FALLBACKS):
-            self.fallback_position.insert(0, self.current_position)
-            self.fallback_position.pop()
+            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()
 
-    def navigate_to_fallback(self, index):
-        assert(index < self.DISTANCE_BETWEEN_FALLBACKS)
+
+    def navigate_to_fallback(self):
         # Prepare the goal message and send it
         goal_pose = PoseStamped()
         goal_pose.header = Header(frame_id='map')
 
-        goal_pose.pose.position = self.fallback_position[index]
+        goal_pose.pose.position = self.fallback_positions[self.fallbackIndex]
         goal_pose.pose.orientation.w = 1.0  # Assuming no orientation preference
 
-        self.get_logger().info(f'Navigating to fallback at ({self.fallback_position.__str__()})')
+        self.get_logger().warn(f'Navigating to fallback at ({self.fallback_positions[self.fallbackIndex].__str__()})')
         self.action_client.wait_for_server()
-        self.send_goal(goal_pose)
-
-    def send_goal(self, goal_pose):
+        
         goal_msg = NavigateToPose.Goal()
         goal_msg.pose = goal_pose
-        self.goal_handle = self.action_client.send_goal_async(goal_msg,self.fallback_point_reached).result()
-        self.get_logger().info('Goal sent to navigation system.')
-        
-    def fallback_point_reached(self):
-        if self.state == self.OFFLINE:
+        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):
+            self.get_logger().warn(f'Fallback {self.fallbackIndex} reached. Will attempt to reach next one if still offline.')
             self.fallbackIndex+=1
-            self.navigate_to_fallback(self.fallbackIndex)
+        if self.state == self.OFFLINE and self.fallbackIndex >= len(self.fallback_positions):
+            self.state = self.WAITING
+            self.get_logger().error(f'Last fallback reached, waiting and hoping for connection to come back :(')
+        
+    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)
+        # Sets the first position
+        if len(self.fallback_positions) == 0: 
+            self.fallback_positions = [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)
+            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]
+
+                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}')            
 
 def main(args=None):
     rclpy.init(args=args)

src/rove_navigation/setup.py

@@ -30,6 +30,7 @@
             'person_following = rove_navigation.person_following:main',
             'frontier_publisher = rove_navigation.frontier_publisher:main',
             'exploration = rove_navigation.exploration:main',
+            'lost_connection = rove_navigation.lost_connection:main',
         ],
     },
 )