Skip to content

Commit 4605fe2

Browse files
committed
propotype semi-fonctionel
1 parent 5e9be41 commit 4605fe2

File tree

1 file changed

+96
-82
lines changed

1 file changed

+96
-82
lines changed

src/rove_navigation/rove_navigation/lost_connection.py

Lines changed: 96 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -1,29 +1,33 @@
1-
from asyncio import Future
1+
import time
22
import socket
33
from threading import Thread
4+
5+
from sympy import Point
46
import rclpy
7+
from rclpy.duration import Duration
58
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
1010
import numpy as np
1111
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
1315

1416
# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection
1517
class LostNetworkFallback(Node):
1618

1719
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
2126

2227
self.ONLINE = 'ONLINE'
2328
self.OFFLINE = 'OFFLINE'
2429

25-
self.state = self.ONLINE
26-
self.traveling_to_fallback = False
30+
self.state = self.OFFLINE
2731
self.nbFailedConns = 0
2832
self.fallbackIndex = 0
2933

@@ -32,106 +36,116 @@ def __init__(self):
3236
self.create_timer(self.PING_RATE, self.fallback_behavior)
3337
self.odometry_subscription = self.create_subscription(
3438
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)
4039

41-
self.fallback_positions.pop()
42-
self.current_position: Point
4340
# Thread to execute goal pose posting (because send_goal_async()'s callback was called before reaching actual goal pose)
4441
self.connection_thread = Thread(target=self.check_connection)
4542
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)
4659

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+
5264
# Check connection to google.com TODO: check connection to UI user instead
5365
# Gets called on a different thread
5466
def check_connection(self):
5567
while True:
68+
start = time.time()
5669
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))
5972
# if this following code runs, the connection was established
6073
self.nbFailedConns = 0
6174
if self.state != self.ONLINE:
6275
self.get_logger().info('Connection re-established to server! terminating fallback state')
63-
self.fallback_positions[self.fallbackIndex:]
64-
self.fallbackIndex = 0
6576
self.state = self.ONLINE
66-
except socket.error:
77+
except socket.error as e:
6778
self.nbFailedConns += 1
6879
if self.nbFailedConns >= self.NB_MAX_FAILED_CONNS:
6980
self.state = self.OFFLINE
7081
if self.nbFailedConns == self.NB_MAX_FAILED_CONNS:
7182
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()
7392

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__()})')
8194

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():
89106

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
95112

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
99120

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')
104129

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-
116130
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
123140
# 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]
131145

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)}')
135149

136150
def main(args=None):
137151
rclpy.init(args=args)

0 commit comments

Comments
 (0)