1
- from asyncio import Future
1
+ import time
2
2
import socket
3
3
from threading import Thread
4
+
5
+ from sympy import Point
4
6
import rclpy
7
+ from rclpy .duration import Duration
5
8
from rclpy .node import Node
6
- from rclpy .action import ActionClient
7
- from geometry_msgs .msg import PoseArray
8
- from geometry_msgs .msg import Point , PoseStamped
9
- from nav2_msgs .action import NavigateToPose
9
+ from geometry_msgs .msg import PoseStamped
10
10
import numpy as np
11
11
from std_msgs .msg import Header
12
- from nav_msgs .msg import Odometry
12
+ from nav_msgs .msg import Odometry , Path
13
+ from nav2_msgs .action import NavigateThroughPoses
14
+ from nav2_simple_commander .robot_navigator import BasicNavigator , TaskResult
13
15
14
16
# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
15
17
class LostNetworkFallback (Node ):
16
18
17
19
def __init__ (self ):
18
- self .DISTANCE_BETWEEN_FALLBACKS = 0.4 # meters
19
- self .NB_MAX_FAILED_CONNS = 3
20
- self .PING_RATE = 1 # seconds
20
+ self .DISTANCE_BETWEEN_FALLBACKS = 0.2 # meters
21
+ self .NB_MAX_FAILED_CONNS = 5
22
+ self .PING_RATE = 1.0 # seconds
23
+ self .PING_TIMEOUT = 2 # seconds
24
+ self .ADDRESS_TO_PING = '8.8.8.8'
25
+ self .PORT_TO_PING = 53
21
26
22
27
self .ONLINE = 'ONLINE'
23
28
self .OFFLINE = 'OFFLINE'
24
29
25
- self .state = self .ONLINE
26
- self .traveling_to_fallback = False
30
+ self .state = self .OFFLINE
27
31
self .nbFailedConns = 0
28
32
self .fallbackIndex = 0
29
33
@@ -32,106 +36,116 @@ def __init__(self):
32
36
self .create_timer (self .PING_RATE , self .fallback_behavior )
33
37
self .odometry_subscription = self .create_subscription (
34
38
Odometry , '/odometry/local' , self .odometry_callback , 10 )
35
- self .action_client = ActionClient (self , NavigateToPose , 'navigate_to_pose' )
36
-
37
- #list of potential fallback positions, ascending order
38
- self .fallback_positions = [Point (x = 0.0 ,y = 0.0 ,z = 0.0 )]
39
- self .current_position = Point (x = 0.0 ,y = 0.0 ,z = 0.0 )
40
39
41
- self .fallback_positions .pop ()
42
- self .current_position : Point
43
40
# Thread to execute goal pose posting (because send_goal_async()'s callback was called before reaching actual goal pose)
44
41
self .connection_thread = Thread (target = self .check_connection )
45
42
self .connection_thread .start ()
43
+
44
+ # Navigator object for nav2
45
+ self .navigator = BasicNavigator ()
46
+
47
+ # list of potential fallback positions, ascending order
48
+ self .fallback_path = Path ()
49
+ self .fallback_path .header = Header (frame_id = 'map' )
50
+
51
+ self .current_position = PoseStamped ()
52
+ self .current_position .header .frame_id = 'map'
53
+ self .current_position .header .stamp = self .navigator .get_clock ().now ().to_msg ()
54
+ self .current_position .pose .position .x = 0.0
55
+ self .current_position .pose .position .y = 0.0
56
+ self .current_position .pose .position .z = 0.0
57
+ self .current_position .pose .orientation .w = 1.0
58
+ self .fallback_path .poses .append (self .current_position )
46
59
47
- # gets called on a ros timer (main thread)
48
- def fallback_behavior (self ):
49
- if self .state == self .OFFLINE and self .fallbackIndex < len (self .fallback_positions ):
50
- self .navigate_to_fallback ()
51
-
60
+ self .ini_pos = False
61
+ self .poses_remaining = 0
62
+
63
+
52
64
# Check connection to google.com TODO: check connection to UI user instead
53
65
# Gets called on a different thread
54
66
def check_connection (self ):
55
67
while True :
68
+ start = time .time ()
56
69
try :
57
- socket .setdefaulttimeout (1 ) # in seconds
58
- socket .socket (socket .AF_INET , socket .SOCK_STREAM ).connect (("8.8.8.8" , 53 ))
70
+ socket .setdefaulttimeout (self . PING_TIMEOUT )
71
+ socket .socket (socket .AF_INET , socket .SOCK_STREAM ).connect ((self . ADDRESS_TO_PING , self . PORT_TO_PING ))
59
72
# if this following code runs, the connection was established
60
73
self .nbFailedConns = 0
61
74
if self .state != self .ONLINE :
62
75
self .get_logger ().info ('Connection re-established to server! terminating fallback state' )
63
- self .fallback_positions [self .fallbackIndex :]
64
- self .fallbackIndex = 0
65
76
self .state = self .ONLINE
66
- except socket .error :
77
+ except socket .error as e :
67
78
self .nbFailedConns += 1
68
79
if self .nbFailedConns >= self .NB_MAX_FAILED_CONNS :
69
80
self .state = self .OFFLINE
70
81
if self .nbFailedConns == self .NB_MAX_FAILED_CONNS :
71
82
self .get_logger ().warn (f'Connection Error!!! ping to server failed { self .nbFailedConns } ' +
72
- 'times in a row, activating fallback state, the robot will try to retrace its steps.' )
83
+ 'times in a row, activating fallback state, the robot will try to retrace its steps.' )
84
+ delay = time .time () - start
85
+ if (delay < self .PING_RATE ): time .sleep (delay )
86
+
87
+ # gets called on a ros timer (main thread)
88
+ def fallback_behavior (self ):
89
+ if self .state == self .OFFLINE and self .poses_remaining > 0 :
90
+ self .get_logger ().info ('nb poses remaingin: ' + str (self .poses_remaining ))
91
+ self .navigate_poses ()
73
92
74
- def navigate_to_fallback (self ):
75
- # Prepare the goal message and send it
76
- goal_pose = PoseStamped ()
77
- goal_pose .header = Header (frame_id = 'map' )
78
-
79
- goal_pose .pose .position = self .fallback_positions [self .fallbackIndex ]
80
- goal_pose .pose .orientation .w = 1.0 # Assuming no orientation preference
93
+ # self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})')
81
94
82
- self .get_logger ().warn (f'Navigating to fallback at ({ self .fallback_positions [self .fallbackIndex ].__str__ ()} )' )
83
- self .action_client .wait_for_server ()
84
-
85
- goal_msg = NavigateToPose .Goal ()
86
- goal_msg .pose = goal_pose
87
- future = self .action_client .send_goal_async (goal_msg ) ## the future returned from this does NOT WORK >:(
88
- goal_handle = future .result ()
95
+ # self.fallbackIndex+=1
96
+ # if self.fallbackIndex < len(self.fallback_path.poses):
97
+ # self.get_logger().warn(f'Fallback {self.fallbackIndex} reached. Will attempt to reach next one if still offline.')
98
+
99
+ # if self.fallbackIndex == len(self.fallback_path.poses):
100
+ # self.get_logger().error(f'Last fallback reached, waiting and hoping for connection to come back :(')
101
+
102
+ def navigate_poses (self ):
103
+ self .navigator .goThroughPoses (self .fallback_path .poses )
104
+ i = 0
105
+ while not self .navigator .isTaskComplete ():
89
106
90
- while not self .is_robot_close_to_goal () :
91
- if self . state == self .ONLINE :
92
- # TODO: cancel current state of general state_handler, right now only cancels return to fallback
93
- self .action_client . _cancel_goal_async ( goal_handle )
94
- return # end fallback loop
107
+ if self .state == self . ONLINE :
108
+ self .fallbackIndex = 0
109
+ self . navigator . cancelTask ()
110
+ self .fallback_path [ self . fallbackIndex :]
111
+ return
95
112
96
- self .fallbackIndex += 1
97
- if self .fallbackIndex < len (self .fallback_positions ):
98
- self .get_logger ().warn (f'Fallback { self .fallbackIndex } reached. Will attempt to reach next one if still offline.' )
113
+ feedback :NavigateThroughPoses .Feedback = self .navigator .getFeedback ()
114
+ self .poses_remaining = feedback .number_of_poses_remaining
115
+ if feedback and i % 5 == 0 :
116
+ # navigation timeout
117
+ if Duration .from_msg (feedback .navigation_time ) > Duration (seconds = 600.0 ):
118
+ self .navigator .cancelTask ()
119
+ i += 1
99
120
100
- if self .fallbackIndex == len (self .fallback_positions ):
101
- self .get_logger ().error (f'Last fallback reached, waiting and hoping for connection to come back :(' )
102
-
103
- self .traveling_to_fallback = False
121
+ # Do something depending on the return code
122
+ result = self .navigator .getResult ()
123
+ if result == TaskResult .SUCCEEDED :
124
+ print ('Goal succeeded!' )
125
+ elif result == TaskResult .FAILED :
126
+ self .get_logger ().error (f'Couldn\' t proceed through fallbacks, reason unknown' )
127
+ else :
128
+ self .get_logger ().warn (f'nav2 navigator task returned with unknown TaskResult' )
104
129
105
-
106
- def is_robot_close_to_goal (self ):
107
- fall = self .fallback_positions [self .fallbackIndex ]
108
- dist = [self .current_position .x - fall .x ,
109
- self .current_position .y - fall .y ,
110
- self .current_position .z - fall .z ]
111
- self .get_logger ().info ('fallback_positions[i]: ' + str (fall ))
112
- self .get_logger ().info ('self.current_position: ' + str (self .current_position ))
113
- self .get_logger ().info ('distance: ' + str (np .linalg .norm (dist )))
114
- return np .linalg .norm (dist ) <= 0.2
115
-
116
130
def odometry_callback (self , msg : Odometry ):
117
- self .current_position = Point (x = msg .pose .pose .position .x , y = msg .pose .pose .position .y , z = msg .pose .pose .position .z )
118
- # Sets the first position
119
- if len (self .fallback_positions ) == 0 :
120
- self .fallback_positions = [self .current_position ]
121
- self .get_logger ().info (f'First fallback position initiated: { self .fallback_positions [0 ]} ' )
122
-
131
+ self .current_position = PoseStamped ()
132
+ self .current_position .pose .position = msg .pose .pose .position
133
+ self .current_position .pose .orientation = msg .pose .pose .orientation
134
+ self .current_position .header .frame_id = 'map'
135
+ self .current_position .header .stamp = self .navigator .get_clock ().now ().to_msg ()
136
+
137
+ if not self .ini_pos :
138
+ self .navigator .setInitialPose (self .current_position )
139
+ self .ini_pos = True
123
140
# If robot at self.DISTANCE_BETWEEN_FALLBACKS distance or more from last fallback, add new fallback point
124
- if (self .state == self .ONLINE ):
125
- if len (self .fallback_positions ) == 0 :
126
- self .fallback_positions .append (self .current_position )
127
- else :
128
- dist = [self .current_position .x - self .fallback_positions [0 ].x ,
129
- self .current_position .y - self .fallback_positions [0 ].y ,
130
- self .current_position .z - self .fallback_positions [0 ].z ]
141
+ elif (self .state == self .ONLINE ):
142
+ dist = [self .current_position .pose .position .x - self .fallback_path .poses [0 ].pose .position .x ,
143
+ self .current_position .pose .position .y - self .fallback_path .poses [0 ].pose .position .y ,
144
+ self .current_position .pose .position .z - self .fallback_path .poses [0 ].pose .position .z ]
131
145
132
- if (np .linalg .norm (dist ) >= self .DISTANCE_BETWEEN_FALLBACKS ):
133
- self .fallback_positions .insert (0 , self .current_position )
134
- self .get_logger ().info (f'New fallback position added: { self .fallback_positions [0 ].__str__ ()} total fallback points: { len (self .fallback_positions )} ' )
146
+ if (np .linalg .norm (dist ) >= self .DISTANCE_BETWEEN_FALLBACKS ):
147
+ self .fallback_path . poses .insert (0 , self .current_position )
148
+ self .get_logger ().info (f'New fallback position added: { self .fallback_path . poses [0 ].__str__ ()} total fallback points: { len (self .fallback_path . poses )} ' )
135
149
136
150
def main (args = None ):
137
151
rclpy .init (args = args )
0 commit comments