From ae5e38ecdeedcf41eba14f94feacb2a6b18c51f2 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Sat, 22 Jun 2024 08:52:37 -0400 Subject: [PATCH 01/12] broken a bit but hey --- src/rove_zed/config/zed_body_trck.yaml | 2 +- src/rove_zed/launch/zed_body_trck.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rove_zed/config/zed_body_trck.yaml b/src/rove_zed/config/zed_body_trck.yaml index e421a25..cd62cfa 100644 --- a/src/rove_zed/config/zed_body_trck.yaml +++ b/src/rove_zed/config/zed_body_trck.yaml @@ -7,7 +7,7 @@ body_format: "BODY_18" # 'BODY_18','BODY_34','BODY_38','BODY_70' allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage max_range: 20.0 # [m] Defines a upper depth range for detections - body_kp_selection: "UPPER_BODY" # 'FULL', 'UPPER_BODY' + body_kp_selection: "FULL" # 'FULL', 'UPPER_BODY' enable_body_fitting: false # Defines if the body fitting will be applied enable_tracking: true # Defines if the object detection will track objects across images flow prediction_timeout_s: 3.0 # During this time [sec], the skeleton will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index 657b8a9..f3c050b 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): - camera_model = 'zedm' + camera_model = 'zed2i' pkg_zed_wrapper = get_package_share_directory('zed_wrapper') launch_file_path = os.path.join(pkg_zed_wrapper, 'launch', 'zed_camera.launch.py') From b85029ebce6364e05ebee684a1c412354b938b2c Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Sun, 23 Jun 2024 09:42:53 -0400 Subject: [PATCH 02/12] Implemented pose tracking to vcs and adapted launch file for zed_body_trck --- rove.repos | 4 ++++ src/rove_zed/launch/zed_body_trck.launch.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/rove.repos b/rove.repos index c628c9d..8eb12b9 100644 --- a/rove.repos +++ b/rove.repos @@ -1,4 +1,8 @@ repositories: + capra_pose_tracking: + type: git + url: https://github.com/clubcapra/capra-pose-tracking + version: master ovis_ros2: type: git url: https://github.com/clubcapra/ovis_ros2 diff --git a/src/rove_zed/launch/zed_body_trck.launch.py b/src/rove_zed/launch/zed_body_trck.launch.py index f3c050b..e452a81 100644 --- a/src/rove_zed/launch/zed_body_trck.launch.py +++ b/src/rove_zed/launch/zed_body_trck.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): launch_description_source=launch_file_path, launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` ['camera_model', camera_model], - ['camera_name', camera_model], + ['camera_name', 'zed'], ['ros_params_override_path', config_override_path] ] ) From 52f592cf8292894765bb0604463d6fb8a9a030d7 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Sun, 23 Jun 2024 17:45:11 +0200 Subject: [PATCH 03/12] untested behavior --- .../rove_navigation/lost_connection.py | 95 +++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 src/rove_navigation/rove_navigation/lost_connection.py diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py new file mode 100644 index 0000000..b88793a --- /dev/null +++ b/src/rove_navigation/rove_navigation/lost_connection.py @@ -0,0 +1,95 @@ +import socket +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 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 + self.NB_MAX_FAILED_CONNS = 3 + self.nbFailedConns = 0 + self.fallbackIndex = 0 + self.PING_RATE = 1 # seconds + + self.ONLINE = 'ONLINE' + self.OFFLINE = 'OFFLINE' + + self.state = constants.OFFLINE + self.goal_handle = None + + super().__init__('lost_network_fallback') + 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') + + #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) + + # 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: + 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() + + def navigate_to_fallback(self, index): + assert(index < self.DISTANCE_BETWEEN_FALLBACKS) + # 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.orientation.w = 1.0 # Assuming no orientation preference + + self.get_logger().info(f'Navigating to fallback at ({self.fallback_position.__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: + self.fallbackIndex+=1 + self.navigate_to_fallback(self.fallbackIndex) + +def main(args=None): + rclpy.init(args=args) + node = LostNetworkFallback() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() From cfc4aa47adc37bfb8f9d01d6f2013cd950ab7f85 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Sun, 23 Jun 2024 23:53:53 +0200 Subject: [PATCH 04/12] 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', ], }, ) From 7ad7da515d3d01349e3b4fe7db21aedb0c42210a Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Mon, 24 Jun 2024 15:15:09 +0200 Subject: [PATCH 05/12] fonctionne pas --- .../rove_navigation/lost_connection.py | 122 ++++++++++-------- 1 file changed, 69 insertions(+), 53 deletions(-) diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index 74fefc1..d4a3daf 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/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) From 5e9be4119babaadce337203453d84274c785e9dd Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Mon, 24 Jun 2024 15:15:46 +0200 Subject: [PATCH 06/12] fonctionne pas fix --- .../rove_navigation/lost_connection.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index d4a3daf..0e3d83b 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/src/rove_navigation/rove_navigation/lost_connection.py @@ -45,9 +45,9 @@ def __init__(self): self.connection_thread.start() # gets called on a ros timer (main thread) - async def fallback_behavior(self): + def fallback_behavior(self): if self.state==self.OFFLINE and self.fallbackIndex < len(self.fallback_positions): - await self.navigate_to_fallback() + self.navigate_to_fallback() # Check connection to google.com TODO: check connection to UI user instead # Gets called on a different thread @@ -71,7 +71,7 @@ def check_connection(self): 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.') - async def navigate_to_fallback(self): + def navigate_to_fallback(self): # Prepare the goal message and send it goal_pose = PoseStamped() goal_pose.header = Header(frame_id='map') @@ -84,7 +84,9 @@ async def navigate_to_fallback(self): goal_msg = NavigateToPose.Goal() goal_msg.pose = goal_pose - goal_handle = await self.action_client.send_goal(goal_msg) ## the future returned from this does NOT WORK >:( + future = self.action_client.send_goal_async(goal_msg) ## the future returned from this does NOT WORK >:( + goal_handle = future.result() + 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 @@ -109,7 +111,7 @@ def is_robot_close_to_goal(self): 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 + return np.linalg.norm(dist) <= 0.2 def odometry_callback(self, msg : Odometry): self.current_position = Point(x=msg.pose.pose.position.x, y=msg.pose.pose.position.y, z=msg.pose.pose.position.z) From 4605fe2b737ad20d9e582c938e007830a2788735 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Tue, 25 Jun 2024 23:23:17 +0200 Subject: [PATCH 07/12] propotype semi-fonctionel --- .../rove_navigation/lost_connection.py | 178 ++++++++++-------- 1 file changed, 96 insertions(+), 82 deletions(-) diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index 0e3d83b..7365528 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/src/rove_navigation/rove_navigation/lost_connection.py @@ -1,29 +1,33 @@ -from asyncio import Future +import time import socket from threading import Thread + +from sympy import Point import rclpy +from rclpy.duration import Duration from rclpy.node import Node -from rclpy.action import ActionClient -from geometry_msgs.msg import PoseArray -from geometry_msgs.msg import Point, PoseStamped -from nav2_msgs.action import NavigateToPose +from geometry_msgs.msg import PoseStamped import numpy as np from std_msgs.msg import Header -from nav_msgs.msg import Odometry +from nav_msgs.msg import Odometry, Path +from nav2_msgs.action import NavigateThroughPoses +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult # 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.DISTANCE_BETWEEN_FALLBACKS = 0.4 # meters - self.NB_MAX_FAILED_CONNS = 3 - self.PING_RATE = 1 # seconds + self.DISTANCE_BETWEEN_FALLBACKS = 0.2 # meters + self.NB_MAX_FAILED_CONNS = 5 + self.PING_RATE = 1.0 # seconds + self.PING_TIMEOUT = 2 # seconds + self.ADDRESS_TO_PING = '8.8.8.8' + self.PORT_TO_PING = 53 self.ONLINE = 'ONLINE' self.OFFLINE = 'OFFLINE' - self.state = self.ONLINE - self.traveling_to_fallback = False + self.state = self.OFFLINE self.nbFailedConns = 0 self.fallbackIndex = 0 @@ -32,106 +36,116 @@ def __init__(self): 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() + + # Navigator object for nav2 + self.navigator = BasicNavigator() + + # list of potential fallback positions, ascending order + self.fallback_path = Path() + self.fallback_path.header = Header(frame_id='map') + + self.current_position = PoseStamped() + self.current_position.header.frame_id = 'map' + self.current_position.header.stamp = self.navigator.get_clock().now().to_msg() + self.current_position.pose.position.x = 0.0 + self.current_position.pose.position.y = 0.0 + self.current_position.pose.position.z = 0.0 + self.current_position.pose.orientation.w = 1.0 + self.fallback_path.poses.append(self.current_position) - # gets called on a ros timer (main thread) - def fallback_behavior(self): - if self.state==self.OFFLINE and self.fallbackIndex < len(self.fallback_positions): - self.navigate_to_fallback() - + self.ini_pos = False + self.poses_remaining = 0 + + # Check connection to google.com TODO: check connection to UI user instead # Gets called on a different thread def check_connection(self): while True: + start = time.time() try: - socket.setdefaulttimeout(1) # in seconds - socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect(("8.8.8.8", 53)) + socket.setdefaulttimeout(self.PING_TIMEOUT) + socket.socket(socket.AF_INET, socket.SOCK_STREAM).connect((self.ADDRESS_TO_PING, self.PORT_TO_PING)) # 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: + except socket.error as e: 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.') + 'times in a row, activating fallback state, the robot will try to retrace its steps.') + delay = time.time() - start + if(delay < self.PING_RATE): time.sleep(delay) + + # gets called on a ros timer (main thread) + def fallback_behavior(self): + if self.state==self.OFFLINE and self.poses_remaining > 0: + self.get_logger().info('nb poses remaingin: '+ str(self.poses_remaining)) + self.navigate_poses() - 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_positions[self.fallbackIndex] - goal_pose.pose.orientation.w = 1.0 # Assuming no orientation preference + # self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})') - self.get_logger().warn(f'Navigating to fallback at ({self.fallback_positions[self.fallbackIndex].__str__()})') - self.action_client.wait_for_server() - - goal_msg = NavigateToPose.Goal() - goal_msg.pose = goal_pose - future = self.action_client.send_goal_async(goal_msg) ## the future returned from this does NOT WORK >:( - goal_handle = future.result() + # self.fallbackIndex+=1 + # if self.fallbackIndex < len(self.fallback_path.poses): + # self.get_logger().warn(f'Fallback {self.fallbackIndex} reached. Will attempt to reach next one if still offline.') + + # if self.fallbackIndex == len(self.fallback_path.poses): + # self.get_logger().error(f'Last fallback reached, waiting and hoping for connection to come back :(') + + def navigate_poses(self): + self.navigator.goThroughPoses(self.fallback_path.poses) + i = 0 + while not self.navigator.isTaskComplete(): - 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 + if self.state == self.ONLINE: + self.fallbackIndex = 0 + self.navigator.cancelTask() + self.fallback_path[self.fallbackIndex:] + return - 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.') + feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback() + self.poses_remaining = feedback.number_of_poses_remaining + if feedback and i % 5 == 0: + # navigation timeout + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + self.navigator.cancelTask() + i+=1 - 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 + # Do something depending on the return code + result = self.navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.FAILED: + self.get_logger().error(f'Couldn\'t proceed through fallbacks, reason unknown') + else : + self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult') - - 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.2 - def odometry_callback(self, msg : Odometry): - 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 = [self.current_position] - self.get_logger().info(f'First fallback position initiated: {self.fallback_positions[0]}') - + self.current_position = PoseStamped() + self.current_position.pose.position = msg.pose.pose.position + self.current_position.pose.orientation = msg.pose.pose.orientation + self.current_position.header.frame_id = 'map' + self.current_position.header.stamp = self.navigator.get_clock().now().to_msg() + + if not self.ini_pos: + self.navigator.setInitialPose(self.current_position) + self.ini_pos = True # 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(self.current_position) - else: - 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] + elif(self.state == self.ONLINE): + dist = [self.current_position.pose.position.x - self.fallback_path.poses[0].pose.position.x, + self.current_position.pose.position.y - self.fallback_path.poses[0].pose.position.y, + self.current_position.pose.position.z - self.fallback_path.poses[0].pose.position.z] - if(np.linalg.norm(dist) >= self.DISTANCE_BETWEEN_FALLBACKS): - 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)}') + if(np.linalg.norm(dist) >= self.DISTANCE_BETWEEN_FALLBACKS): + self.fallback_path.poses.insert(0, self.current_position) + self.get_logger().info(f'New fallback position added: {self.fallback_path.poses[0].__str__()} total fallback points: {len(self.fallback_path.poses)}') def main(args=None): rclpy.init(args=args) From 5cb3cf64c7d180829bcc0476960b30dd6d2f96c6 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Wed, 26 Jun 2024 00:31:01 +0200 Subject: [PATCH 08/12] added reverse --- src/rove_navigation/config/rove_nav_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml index 319d890..098d775 100644 --- a/src/rove_navigation/config/rove_nav_params.yaml +++ b/src/rove_navigation/config/rove_nav_params.yaml @@ -129,7 +129,7 @@ controller_server: simulate_ahead_time: 1.0 plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true - min_vel_x: 0.0 + min_vel_x: -0.4 min_vel_y: 0.0 max_vel_x: 0.4 max_vel_y: 0.0 From e19c08a0e0e9c3b072446164d245971c61e38080 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Wed, 26 Jun 2024 14:27:30 +0200 Subject: [PATCH 09/12] APPARENTLY CANT CALL NAVIGATOR IN MULTITHREAD :) --- .../launch/mule_behaviour.launch.py | 33 ++++ .../behavior/mule_constants.py | 13 ++ .../rove_navigation/lost_connection.py | 2 +- .../rove_navigation/mule_behavior.py | 147 ++++++++++++++++++ .../rove_navigation/person_following.py | 59 ++++--- src/rove_navigation/setup.py | 2 + 6 files changed, 231 insertions(+), 25 deletions(-) create mode 100644 src/rove_navigation/launch/mule_behaviour.launch.py create mode 100644 src/rove_navigation/rove_navigation/behavior/mule_constants.py create mode 100644 src/rove_navigation/rove_navigation/mule_behavior.py diff --git a/src/rove_navigation/launch/mule_behaviour.launch.py b/src/rove_navigation/launch/mule_behaviour.launch.py new file mode 100644 index 0000000..6a5c559 --- /dev/null +++ b/src/rove_navigation/launch/mule_behaviour.launch.py @@ -0,0 +1,33 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + +from launch_ros.actions import Node +from launch_ros.actions import SetRemap + +def generate_launch_description(): + + mule_behavior = Node( + package="rove_navigation", + executable='mule_behavior', + name='mule_behavior', + output='screen', + ) + + person_following = Node( + package="rove_navigation", + executable='person_following', + name='navigate_to_person', + output='screen', + ) + + return LaunchDescription([ + #person_following_node, + mule_behavior, + person_following + ]) diff --git a/src/rove_navigation/rove_navigation/behavior/mule_constants.py b/src/rove_navigation/rove_navigation/behavior/mule_constants.py new file mode 100644 index 0000000..07d0752 --- /dev/null +++ b/src/rove_navigation/rove_navigation/behavior/mule_constants.py @@ -0,0 +1,13 @@ +# states +START = 'START' # At the beginning, exiting this state sets the startpoint +FOLLOWING = 'FOLLOWING' # make person_following behaviour active +REWIND = 'REWIND' # Initiates the rewind phase +PROCESSED_REWIND = 'PROCESSED_REWIND' # Continues the rewind phase until another TPOSE is detected +PAUSE = 'PAUSE' # Freezes the robot in place (TODO: nice to have, make it still oriente itself towards the target, but not move) +END = 'END' + +# poses +FIGURE_IDLE = "IDLE" # Continue state +FIGURE_TPOSE = "TPOSE" # Start the follow +FIGURE_BUCKET = "BUCKET" # Pause/UnPause +FIGURE_UP = "UP" # start the rewind phase \ No newline at end of file diff --git a/src/rove_navigation/rove_navigation/lost_connection.py b/src/rove_navigation/rove_navigation/lost_connection.py index 7365528..dd40fec 100644 --- a/src/rove_navigation/rove_navigation/lost_connection.py +++ b/src/rove_navigation/rove_navigation/lost_connection.py @@ -87,7 +87,7 @@ def check_connection(self): # gets called on a ros timer (main thread) def fallback_behavior(self): if self.state==self.OFFLINE and self.poses_remaining > 0: - self.get_logger().info('nb poses remaingin: '+ str(self.poses_remaining)) + self.get_logger().info('nb poses remaining: '+ str(self.poses_remaining)) self.navigate_poses() # self.get_logger().warn(f'Navigating to fallback at ({self.fallback_path[self.fallbackIndex].__str__()})') diff --git a/src/rove_navigation/rove_navigation/mule_behavior.py b/src/rove_navigation/rove_navigation/mule_behavior.py new file mode 100644 index 0000000..b378c9b --- /dev/null +++ b/src/rove_navigation/rove_navigation/mule_behavior.py @@ -0,0 +1,147 @@ +import time +import socket +from threading import Thread + +from sympy import Point +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +import numpy as np +from std_msgs.msg import String, Header +from nav_msgs.msg import Odometry, Path +from nav2_msgs.action import NavigateThroughPoses +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rove_navigation.behavior.mule_constants as consts + +# Node to handle when the robot loses connection with internet, stores waypoints to fall back to to try to get back connection +class MuleBehavior(Node): + def __init__(self): + super().__init__('mule_behavior') + self.subscription_person_position = self.create_subscription( + String, + '/tracking/state', + self.state_listener, + 10) + self.pub_mule_state = self.create_publisher( + String, + '/mule_state', + 10) + self.odom_subscriber = self.create_subscription( + Odometry, + '/odometry/local', + self.odom_listener, + 10) + + # nav variables + self.startPoint: PoseStamped + self.endpoint: PoseStamped + self.goal_point: PoseStamped + self.currentPos: PoseStamped + self.navigator = BasicNavigator() + + + self.state = '' + self.change_state(consts.START) + self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed + + self.get_logger().info('Initiating MuleBehavior node') + + # Thread to execute goal pose posting for rewind + self.connection_thread = Thread(target=self.rewind_behavior) + self.connection_thread.start() + + def change_state(self, state): + if(state != self.state): + self.state = state + self.pub_mule_state.publish(String(data=state)) + + # Subscribed to /odometry/local + def odom_listener(self, msg:Odometry): + self.currentPos = PoseStamped() + self.currentPos.header.frame_id = msg.header.frame_id + self.currentPos.header.stamp = msg.header.stamp + self.currentPos.pose = msg.pose.pose + + # Subscribed to /tracking/state, handles state/figure changes + def state_listener(self, msg:String): + if self.state != msg.data: + returnState = msg.data + match msg.data: + case consts.FIGURE_TPOSE: + if self.state == consts.START: + self.startPoint = self.currentPos + self.navigator.setInitialPose(self.startPoint) + self.get_logger().info(f'Mule behavior engaged, point A noted at coordonates {self.startPoint.pose.position}') + returnState = consts.FOLLOWING + + case consts.FIGURE_IDLE: + if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND: + returnState=self.state + + case consts.FIGURE_UP: + if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND: + if self.state == consts.PAUSE: returnState = consts.FOLLOWING + else: returnState = consts.PAUSE + + case consts.FIGURE_BUCKET: + if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND: + self.endpoint = self.currentPos + returnState = consts.REWIND + + case _: + self.get_logger().error(f'Unimplemented pose returned: {msg.data}') + + self.change_state(returnState) + + # runs on a thread continuously + def rewind_behavior(self): + while self.state != consts.END: + if self.state == consts.REWIND: + self.get_logger().info(f'Rewind phase engaged, point B noted at coordonates {self.endpoint.pose.position}') + self.change_state(consts.PROCESSED_REWIND) + self.goal_point = self.startPoint + while self.state == consts.PROCESSED_REWIND: + self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed + result = self.navigator.getResult() + goal = 'A' + nextgoal = 'B' + if self.goal_point == self.startPoint: + goal = 'B' + nextgoal = 'A' + if result == TaskResult.SUCCEEDED: + self.get_logger().info(f'Goal {goal} reached! Initiating nav towards point {nextgoal}') + self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint + elif result == TaskResult.FAILED: + self.get_logger().error(f'Failed to reach {goal}! Initiating nav towards point {nextgoal}') + self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint + elif result == TaskResult.CANCELED: + self.get_logger().error(f'Rewinding to point {goal} was cancelled! Initiating nav towards point {nextgoal}') + else : + self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult') + + # Tells navigator to go to current goal Point and processes feedback + def navigate_to_goal_point(self): + i = 0 + self.navigator.goToPose(self.goal_point) + while not self.navigator.isTaskComplete(): + if self.state != consts.PROCESSED_REWIND: + self.navigator.cancelTask() + + feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback() + self.poses_remaining = feedback.number_of_poses_remaining + if feedback and i % 5 == 0: + # navigation timeout + if Duration.from_msg(feedback.navigation_time) > 1.5 * Duration(seconds=feedback.estimated_time_remaining): + self.navigator.cancelTask() + i+=1 + +def main(args=None): + rclpy.init(args=args) + node = MuleBehavior() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/rove_navigation/rove_navigation/person_following.py b/src/rove_navigation/rove_navigation/person_following.py index 54d905b..053657e 100644 --- a/src/rove_navigation/rove_navigation/person_following.py +++ b/src/rove_navigation/rove_navigation/person_following.py @@ -3,7 +3,9 @@ from geometry_msgs.msg import PointStamped, PoseStamped from tf2_ros import Buffer, TransformListener from tf2_geometry_msgs import PointStamped +from std_msgs.msg import String import math +import rove_navigation.behavior.mule_constants as consts class NavigateToPersonNode(Node): def __init__(self, truncate_distance): @@ -18,37 +20,46 @@ def __init__(self, truncate_distance): PoseStamped, '/goal_pose', 10) - + self.pub_mule_state = self.create_subscription( + String, + '/mule_state', + self.mule_state_listener, + 10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) + self.curr_phase = consts.START # Distance to truncate from the target position self.truncate_distance = truncate_distance + + def mule_state_listener(self, msg: String): + self.curr_phase = msg def navigate_to_person(self, msg): - # Ensure that the transformation is available - now = rclpy.time.Time() - if self.tf_buffer.can_transform('map', msg.header.frame_id, now): - point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1)) - goal_pose = PoseStamped() - goal_pose.header.frame_id = "map" - goal_pose.header.stamp = self.get_clock().now().to_msg() - - # Calculate the angle for the truncated goal using atan2 - angle = math.atan2(point_transformed.point.y, point_transformed.point.x) - - # Calculate the position truncating the specified distance from the transformed position - goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle) - goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle) - goal_pose.pose.position.z = 0.0 # Normally zero for ground robots - - goal_pose.pose.orientation.w = 1.0 # No rotation about the z-axis - - # Log the navigation target for debugging - self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}') - - # Publish the goal - self.goal_update_pub.publish(goal_pose) + if self.curr_phase == consts.FOLLOWING: + # Ensure that the transformation is available + now = rclpy.time.Time() + if self.tf_buffer.can_transform('map', msg.header.frame_id, now): + point_transformed = self.tf_buffer.transform(msg, 'map', timeout=rclpy.duration.Duration(seconds=1)) + goal_pose = PoseStamped() + goal_pose.header.frame_id = "map" + goal_pose.header.stamp = self.get_clock().now().to_msg() + + # Calculate the angle for the truncated goal using atan2 + angle = math.atan2(point_transformed.point.y, point_transformed.point.x) + + # Calculate the position truncating the specified distance from the transformed position + goal_pose.pose.position.x = point_transformed.point.x - self.truncate_distance * math.cos(angle) + goal_pose.pose.position.y = point_transformed.point.y - self.truncate_distance * math.sin(angle) + goal_pose.pose.position.z = 0.0 # Normally zero for ground robots + + goal_pose.pose.orientation.w = 1.0 # No rotation about the z-axis + + # Log the navigation target for debugging + self.get_logger().info(f'Navigating to truncated goal: {goal_pose.pose.position.x}, {goal_pose.pose.position.y}') + + # Publish the goal + self.goal_update_pub.publish(goal_pose) def main(args=None): rclpy.init(args=args) diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index e838a8d..a1245aa 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -22,6 +22,7 @@ description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], + package_dir={'': 'src'}, entry_points={ 'console_scripts': [ 'navigate_to_person = rove_navigation.navigate_to_person:main', @@ -31,6 +32,7 @@ 'frontier_publisher = rove_navigation.frontier_publisher:main', 'exploration = rove_navigation.exploration:main', 'lost_connection = rove_navigation.lost_connection:main', + 'mule_behavior = rove_navigation.mule_behavior:main', ], }, ) From a698781012d85f1638a3f6dfd9837c6644b64883 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Wed, 26 Jun 2024 15:19:51 +0200 Subject: [PATCH 10/12] fuck-le-rewind --- .../rove_navigation/mule_behavior.py | 90 +++++++++++-------- 1 file changed, 54 insertions(+), 36 deletions(-) diff --git a/src/rove_navigation/rove_navigation/mule_behavior.py b/src/rove_navigation/rove_navigation/mule_behavior.py index b378c9b..f1cf0b9 100644 --- a/src/rove_navigation/rove_navigation/mule_behavior.py +++ b/src/rove_navigation/rove_navigation/mule_behavior.py @@ -27,6 +27,15 @@ def __init__(self): String, '/mule_state', 10) + self.goal_update_pub = self.create_publisher( + PoseStamped, + '/goal_pose', + 10) + self.subscription_person_position = self.create_subscription( + String, + '/navigate_to_pose/_action/status', + self.state_listener, + 10) self.odom_subscriber = self.create_subscription( Odometry, '/odometry/local', @@ -43,7 +52,6 @@ def __init__(self): self.state = '' self.change_state(consts.START) - self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed self.get_logger().info('Initiating MuleBehavior node') @@ -87,7 +95,7 @@ def state_listener(self, msg:String): case consts.FIGURE_BUCKET: if self.state != consts.REWIND and self.state != consts.PROCESSED_REWIND: self.endpoint = self.currentPos - returnState = consts.REWIND + returnState = consts.REACH_A case _: self.get_logger().error(f'Unimplemented pose returned: {msg.data}') @@ -97,44 +105,54 @@ def state_listener(self, msg:String): # runs on a thread continuously def rewind_behavior(self): while self.state != consts.END: - if self.state == consts.REWIND: - self.get_logger().info(f'Rewind phase engaged, point B noted at coordonates {self.endpoint.pose.position}') - self.change_state(consts.PROCESSED_REWIND) + if self.state == consts.REACH_A: + self.get_logger().info(f'Going to point A coordonates {self.startpoint.pose.position}') self.goal_point = self.startPoint - while self.state == consts.PROCESSED_REWIND: - self.navigate_to_goal_point() # returns only when goal reached/cancelled/failed - result = self.navigator.getResult() - goal = 'A' - nextgoal = 'B' - if self.goal_point == self.startPoint: - goal = 'B' - nextgoal = 'A' - if result == TaskResult.SUCCEEDED: - self.get_logger().info(f'Goal {goal} reached! Initiating nav towards point {nextgoal}') - self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint - elif result == TaskResult.FAILED: - self.get_logger().error(f'Failed to reach {goal}! Initiating nav towards point {nextgoal}') - self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint - elif result == TaskResult.CANCELED: - self.get_logger().error(f'Rewinding to point {goal} was cancelled! Initiating nav towards point {nextgoal}') - else : - self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult') + self.navigate_to_goal_point() + self.change_state('REWIND') + if self.state == consts.REACH_B: + self.get_logger().info(f'Going to point B coordonates {self.endpoint.pose.position}') + self.goal_point = self.endpoint + self.navigate_to_goal_point() + self.change_state('REWIND') + + # goal = 'A' + # nextgoal = 'B' + # if self.goal_point == self.startPoint: + # goal = 'B' + # nextgoal = 'A' + # if result == TaskResult.SUCCEEDED: + # self.get_logger().info(f'Goal {goal} reached! Initiating nav towards point {nextgoal}') + # self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint + # elif result == TaskResult.FAILED: + # self.get_logger().error(f'Failed to reach {goal}! Initiating nav towards point {nextgoal}') + # self.goal_point = self.startPoint if self.goal_point == self.endpoint else self.endpoint + # elif result == TaskResult.CANCELED: + # self.get_logger().error(f'Rewinding to point {goal} was cancelled! Initiating nav towards point {nextgoal}') + # else : + # self.get_logger().warn(f'nav2 navigator task returned with unknown TaskResult') - # Tells navigator to go to current goal Point and processes feedback def navigate_to_goal_point(self): - i = 0 - self.navigator.goToPose(self.goal_point) - while not self.navigator.isTaskComplete(): - if self.state != consts.PROCESSED_REWIND: - self.navigator.cancelTask() + # Log the navigation target for debugging + self.get_logger().info(f'Navigating to goal : {self.goal_point}') + # Publish the goal + self.goal_update_pub.publish(self.goal_point) + + # # Tells navigator to go to current goal Point and processes feedback + # def navigate_to_goal_point(self): + # i = 0 + # self.navigator.goToPose(self.goal_point) + # while not self.navigator.isTaskComplete(): + # if self.state != consts.PROCESSED_REWIND: + # self.navigator.cancelTask() - feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback() - self.poses_remaining = feedback.number_of_poses_remaining - if feedback and i % 5 == 0: - # navigation timeout - if Duration.from_msg(feedback.navigation_time) > 1.5 * Duration(seconds=feedback.estimated_time_remaining): - self.navigator.cancelTask() - i+=1 + # feedback:NavigateThroughPoses.Feedback = self.navigator.getFeedback() + # self.poses_remaining = feedback.number_of_poses_remaining + # if feedback and i % 5 == 0: + # # navigation timeout + # if Duration.from_msg(feedback.navigation_time) > 1.5 * Duration(seconds=feedback.estimated_time_remaining): + # self.navigator.cancelTask() + # i+=1 def main(args=None): rclpy.init(args=args) From dead2a6cff0e94cb135b7ae8baab305b3d6960d5 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Wed, 26 Jun 2024 15:20:01 +0200 Subject: [PATCH 11/12] constants --- src/rove_navigation/rove_navigation/behavior/mule_constants.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/rove_navigation/rove_navigation/behavior/mule_constants.py b/src/rove_navigation/rove_navigation/behavior/mule_constants.py index 07d0752..1c819ed 100644 --- a/src/rove_navigation/rove_navigation/behavior/mule_constants.py +++ b/src/rove_navigation/rove_navigation/behavior/mule_constants.py @@ -5,6 +5,8 @@ PROCESSED_REWIND = 'PROCESSED_REWIND' # Continues the rewind phase until another TPOSE is detected PAUSE = 'PAUSE' # Freezes the robot in place (TODO: nice to have, make it still oriente itself towards the target, but not move) END = 'END' +REACH_A = 'REACH_A' +REACH_B = 'REACH_B' # poses FIGURE_IDLE = "IDLE" # Continue state From 66dd9f64502b5479ade8f56366f8ce7cb739bee6 Mon Sep 17 00:00:00 2001 From: nathan-gt Date: Wed, 26 Jun 2024 15:47:56 +0200 Subject: [PATCH 12/12] fix topic name --- src/rove_navigation/rove_navigation/person_following.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rove_navigation/rove_navigation/person_following.py b/src/rove_navigation/rove_navigation/person_following.py index 053657e..2bd8ad5 100644 --- a/src/rove_navigation/rove_navigation/person_following.py +++ b/src/rove_navigation/rove_navigation/person_following.py @@ -12,7 +12,7 @@ def __init__(self, truncate_distance): super().__init__('navigate_to_person') self.subscription = self.create_subscription( PointStamped, - '/person_position', + '/tracking/point', self.navigate_to_person, 10)