From cfc4aa47adc37bfb8f9d01d6f2013cd950ab7f85 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Sun, 23 Jun 2024 23:53:53 +0200 Subject: [PATCH] added lost_connection behavior (pain and suffering) --- .../rove_navigation/lost_connection.py | 109 +++++++++++------- src/rove_navigation/setup.py | 1 + 2 files changed, 71 insertions(+), 39 deletions(-) diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index b88793a..74fefc1 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/src/rove_navigation/rove_navigation/lost_connection.py @@ -2,21 +2,18 @@ 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 @@ -24,65 +21,99 @@ def __init__(self): 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) diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index a4ae75a..e838a8d 100644 --- a/src/rove_navigation/setup.py +++ b/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', ], }, )