From 557b1c319915eca15265818e53d32b72b71f53de Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Fri, 30 Jun 2023 12:32:30 +0100 Subject: [PATCH 01/21] dataclass for storing command IDs --- spot_wrapper/wrapper.py | 135 +++++++++++++++++++++++++++++----------- 1 file changed, 97 insertions(+), 38 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 1d99bc86..6e9459de 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -373,10 +373,10 @@ def __init__( self._spot_wrapper: SpotWrapper = spot_wrapper def _start_query(self) -> None: - if self._spot_wrapper._last_stand_command != None: + if self._spot_wrapper.last_stand_command is not None: try: response = self._client.robot_command_feedback( - self._spot_wrapper._last_stand_command + self._spot_wrapper.last_stand_command ) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.stand_feedback.status @@ -384,7 +384,7 @@ def _start_query(self) -> None: self._spot_wrapper.is_sitting = False if status == basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING: self._spot_wrapper.is_standing = True - self._spot_wrapper._last_stand_command = None + self._spot_wrapper.last_stand_command = None elif ( status == basic_command_pb2.StandCommand.Feedback.STATUS_IN_PROGRESS ): @@ -394,38 +394,38 @@ def _start_query(self) -> None: self._spot_wrapper.is_standing = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) - self._spot_wrapper._last_stand_command = None + self._spot_wrapper.last_stand_command = None - if self._spot_wrapper._last_sit_command != None: + if self._spot_wrapper.last_sit_command is not None: try: self._spot_wrapper.is_standing = False response = self._client.robot_command_feedback( - self._spot_wrapper._last_sit_command + self._spot_wrapper.last_sit_command ) if ( response.feedback.synchronized_feedback.mobility_command_feedback.sit_feedback.status == basic_command_pb2.SitCommand.Feedback.STATUS_IS_SITTING ): self._spot_wrapper.is_sitting = True - self._spot_wrapper._last_sit_command = None + self._spot_wrapper.last_sit_command = None else: self._spot_wrapper.is_sitting = False except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) - self._spot_wrapper._last_sit_command = None + self._spot_wrapper.last_sit_command = None is_moving = False - if self._spot_wrapper._last_velocity_command_time != None: - if time.time() < self._spot_wrapper._last_velocity_command_time: + if self._spot_wrapper.last_velocity_command_time is not None: + if time.time() < self._spot_wrapper.last_velocity_command_time: is_moving = True else: - self._spot_wrapper._last_velocity_command_time = None + self._spot_wrapper.last_velocity_command_time = None - if self._spot_wrapper._last_trajectory_command != None: + if self._spot_wrapper.last_trajectory_command is not None: try: response = self._client.robot_command_feedback( - self._spot_wrapper._last_trajectory_command + self._spot_wrapper.last_trajectory_command ) status = ( response.feedback.synchronized_feedback.mobility_command_feedback.se2_trajectory_feedback.status @@ -438,7 +438,7 @@ def _start_query(self) -> None: or ( status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_NEAR_GOAL - and not self._spot_wrapper._last_trajectory_command_precise + and not self._spot_wrapper.last_trajectory_command_precise ) ): self._spot_wrapper.at_goal = True @@ -460,18 +460,18 @@ def _start_query(self) -> None: status == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_UNKNOWN ): - self._spot_wrapper._trajectory_status_unknown = True - self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper.trajectory_status_unknown = True + self._spot_wrapper.last_trajectory_command = None else: self._logger.error( "Received trajectory command status outside of expected range, value is {}".format( status ) ) - self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper.last_trajectory_command = None except (ResponseError, RpcError) as e: self._logger.error("Error when getting robot command feedback: %s", e) - self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper.last_trajectory_command = None self._spot_wrapper.is_moving = is_moving @@ -481,10 +481,10 @@ def _start_query(self) -> None: self._spot_wrapper.is_standing and self._spot_wrapper._continually_try_stand and not self._spot_wrapper.is_moving - and self._spot_wrapper._last_trajectory_command is not None - and self._spot_wrapper._last_stand_command is not None - and self._spot_wrapper._last_velocity_command_time is not None - and self._spot_wrapper._last_docking_command is not None + and self._spot_wrapper.last_trajectory_command is not None + and self._spot_wrapper.last_stand_command is not None + and self._spot_wrapper.last_velocity_command_time is not None + and self._spot_wrapper.last_docking_command is not None ): self._spot_wrapper.stand(False) @@ -575,6 +575,23 @@ class RobotState: near_goal: bool = False +@dataclass() +class RobotCommandData: + """ + Store data about the commands the wrapper sends to the SDK. Running a command returns an integer value + representing that command's ID. These values are used to monitor the progress of the command and modify attributes + of RobotState accordingly. The values should be reset to none when the command completes. + """ + + last_stand_command: typing.Optional[int] = None + last_sit_command: typing.Optional[int] = None + last_docking_command: typing.Optional[int] = None + last_trajectory_command: typing.Optional[int] = None + # Was the last trajectory command requested to be precise + last_trajectory_command_precise: typing.Optional[bool] = None + last_velocity_command_time: typing.Optional[float] = None + + class SpotWrapper: """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically""" @@ -637,13 +654,7 @@ def __init__( self._mobility_params = RobotCommandBuilder.mobility_params() self._state = RobotState() self._trajectory_status_unknown = False - self._last_robot_command_feedback = False - self._last_stand_command = None - self._last_sit_command = None - self._last_trajectory_command = None - self._last_trajectory_command_precise = None - self._last_velocity_command_time = None - self._last_docking_command = None + self._command_data = RobotCommandData() self._front_image_requests = [] for source in front_image_sources: @@ -1122,6 +1133,54 @@ def at_goal(self) -> bool: def at_goal(self, state: bool) -> None: self._state.at_goal = state + @property + def last_stand_command(self) -> typing.Optional[int]: + return self._command_data.last_stand_command + + @last_stand_command.setter + def last_stand_command(self, command_id: int) -> None: + self._command_data.last_stand_command = command_id + + @property + def last_sit_command(self) -> typing.Optional[int]: + return self._command_data.last_sit_command + + @last_sit_command.setter + def last_sit_command(self, command_id: int) -> None: + self._command_data.last_sit_command = command_id + + @property + def last_docking_command(self) -> typing.Optional[int]: + return self._command_data.last_docking_command + + @last_docking_command.setter + def last_docking_command(self, command_id: int) -> None: + self._command_data.last_docking_command = command_id + + @property + def last_trajectory_command(self) -> typing.Optional[int]: + return self._command_data.last_trajectory_command + + @last_trajectory_command.setter + def last_trajectory_command(self, command_id: int) -> None: + self._command_data.last_trajectory_command = command_id + + @property + def last_trajectory_command_precise(self) -> typing.Optional[bool]: + return self._command_data.last_trajectory_command_precise + + @last_trajectory_command_precise.setter + def last_trajectory_command_precise(self, was_precise: bool) -> None: + self._command_data.last_trajectory_command_precise = was_precise + + @property + def last_velocity_command_time(self) -> typing.Optional[float]: + return self._command_data.last_velocity_command_time + + @last_velocity_command_time.setter + def last_velocity_command_time(self, command_time: float) -> None: + self._command_data.last_velocity_command_time = command_time + def is_estopped(self, timeout: typing.Optional[float] = None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -1333,7 +1392,7 @@ def sit(self) -> typing.Tuple[bool, str]: """ response = self._robot_command(RobotCommandBuilder.synchro_sit_command()) - self._last_sit_command = response[2] + self.last_sit_command = response[2] return response[0], response[1] @try_claim(power_on=True) @@ -1348,7 +1407,7 @@ def simple_stand(self, monitor_command: bool = True) -> typing.Tuple[bool, str]: RobotCommandBuilder.synchro_stand_command(params=self._mobility_params) ) if monitor_command: - self._last_stand_command = response[2] + self.last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) @@ -1392,7 +1451,7 @@ def stand( ) if monitor_command: - self._last_stand_command = response[2] + self.last_stand_command = response[2] return response[0], response[1] @try_claim(power_on=True) @@ -1503,7 +1562,7 @@ def velocity_cmd( end_time_secs=end_time, timesync_endpoint=self._robot.time_sync.endpoint, ) - self._last_velocity_command_time = end_time + self.last_velocity_command_time = end_time return response[0], response[1] @try_claim @@ -1538,7 +1597,7 @@ def trajectory_cmd( self._trajectory_status_unknown = False self.at_goal = False self.near_goal = False - self._last_trajectory_command_precise = precise_position + self.last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration if frame_name == "vision": @@ -1580,7 +1639,7 @@ def trajectory_cmd( else: raise ValueError("frame_name must be 'vision' or 'odom'") if response[0]: - self._last_trajectory_command = response[2] + self.last_trajectory_command = response[2] return response[0], response[1] def robot_command( @@ -2697,11 +2756,11 @@ def dock(self, dock_id): self._robot.power_on() self.stand() # Dock the robot - self._last_docking_command = dock_id + self.last_docking_command = dock_id blocking_dock_robot(self._robot, dock_id) - self._last_docking_command = None + self.last_docking_command = None # Necessary to reset this as docking often causes the last stand command to go into an unknown state - self._last_stand_command = None + self.last_stand_command = None return True, "Success" except Exception as e: return False, f"Exception while trying to dock: {e}" From a6fad097b59131c91f5dc8b7899defc1912cb901 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Sun, 2 Jul 2023 11:59:04 +0100 Subject: [PATCH 02/21] missed one trajectory command --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6e9459de..1796c95f 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -443,7 +443,7 @@ def _start_query(self) -> None: ): self._spot_wrapper.at_goal = True # Clear the command once at the goal - self._spot_wrapper._last_trajectory_command = None + self._spot_wrapper.last_trajectory_command = None self._spot_wrapper._trajectory_status_unknown = False elif ( status From 4883e76393c71142885d7d5d041959ede545f67e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 22 Jun 2023 17:43:58 +0900 Subject: [PATCH 03/21] Update _set_initial_localization_waypoint() to support string waypoint_id and add result output. --- spot_wrapper/wrapper.py | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 1796c95f..90f2035f 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2295,22 +2295,31 @@ def _set_initial_localization_fiducial(self, *args): ko_tform_body=current_odom_tform_body, ) - def _set_initial_localization_waypoint(self, *args): - """Trigger localization to a waypoint.""" + def _set_initial_localization_waypoint(self, waypoint_id): + """Trigger localization to a waypoint. + + Args: + waypoint_id (int): ID of waypoint to be localized. + + Returns: + result (bool): success flag of localization + message (str): message about result. + """ # Take the first argument as the localization waypoint. - if len(args) < 1: - # If no waypoint id is given as input, then return without initializing. - self._logger.error("No waypoint specified to initialize to.") - return + if type(waypoint_id) is not str: + if type(waypoint_id) is list and len(waypoint_id) > 1 and type(waypoint_id[0]) is list: + waypoint_id = waypoint_id[0][0] + else: + return False, "No waypoint specified to initialize to." destination_waypoint = graph_nav_util.find_unique_waypoint_id( - args[0][0], + waypoint_id, self._current_graph, self._current_annotation_name_to_wp_id, self._logger, ) if not destination_waypoint: # Failed to find the unique waypoint id. - return + return False, 'Failed to find the unique waypoint id.' robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( @@ -2329,6 +2338,8 @@ def _set_initial_localization_waypoint(self, *args): ko_tform_body=current_odom_tform_body, ) + return True, 'Success' + def _list_graph_waypoint_and_edge_ids(self, *args): """List the waypoint ids and edge ids of the graph currently on the robot.""" From 2f222f8fc1a56822091962d37d0b4a5b10cd9e25 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 19:46:23 +0900 Subject: [PATCH 04/21] Add set_localization_fiducial() and set_localization_waypoint() methods --- spot_wrapper/wrapper.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 90f2035f..cedd4e36 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1753,6 +1753,35 @@ def download_graph(self, download_path: str) -> typing.Tuple[bool, str]: f"Got an error during downloading graph and snapshots from the robot: {e}", ) + def set_localization_fiducial(self) -> tuple(bool, str): + """Localize a robot according to near fiducials. + + Returns: + success (bool): success flag + message (str): message about result + """ + try: + self._set_initial_localization_fiducial() + return True, "Success" + except Exception as e: + return False, f"Error: {e}" + + def set_localization_waypoint(self, waypoint_id: str) -> tuple(bool, str): + """Localize a robot to specified waypoint. + + Args: + waypoint_id (int): ID of waypoint to be localized. + + Returns: + result (bool): success flag of localization + message (str): message about result. + """ + try: + resp = self._set_initial_localization_waypoint(waypoint_id) + return resp[0], resp[1] + except Exception as e: + return False, f"Error: {e}" + @try_claim def navigate_to( self, From 6b01d0454ccb283ff6c3a66a0d5018885d1a5350 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 21:32:25 +0900 Subject: [PATCH 05/21] Fix format and hinting --- spot_wrapper/wrapper.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index cedd4e36..6395d983 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1753,9 +1753,9 @@ def download_graph(self, download_path: str) -> typing.Tuple[bool, str]: f"Got an error during downloading graph and snapshots from the robot: {e}", ) - def set_localization_fiducial(self) -> tuple(bool, str): + def set_localization_fiducial(self) -> typing.Tuple[bool, str]: """Localize a robot according to near fiducials. - + Returns: success (bool): success flag message (str): message about result @@ -1766,9 +1766,9 @@ def set_localization_fiducial(self) -> tuple(bool, str): except Exception as e: return False, f"Error: {e}" - def set_localization_waypoint(self, waypoint_id: str) -> tuple(bool, str): + def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str]: """Localize a robot to specified waypoint. - + Args: waypoint_id (int): ID of waypoint to be localized. @@ -2326,7 +2326,7 @@ def _set_initial_localization_fiducial(self, *args): def _set_initial_localization_waypoint(self, waypoint_id): """Trigger localization to a waypoint. - + Args: waypoint_id (int): ID of waypoint to be localized. @@ -2336,7 +2336,11 @@ def _set_initial_localization_waypoint(self, waypoint_id): """ # Take the first argument as the localization waypoint. if type(waypoint_id) is not str: - if type(waypoint_id) is list and len(waypoint_id) > 1 and type(waypoint_id[0]) is list: + if ( + type(waypoint_id) is list + and len(waypoint_id) > 1 + and type(waypoint_id[0]) is list + ): waypoint_id = waypoint_id[0][0] else: return False, "No waypoint specified to initialize to." @@ -2348,7 +2352,7 @@ def _set_initial_localization_waypoint(self, waypoint_id): ) if not destination_waypoint: # Failed to find the unique waypoint id. - return False, 'Failed to find the unique waypoint id.' + return False, "Failed to find the unique waypoint id." robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( @@ -2367,7 +2371,7 @@ def _set_initial_localization_waypoint(self, waypoint_id): ko_tform_body=current_odom_tform_body, ) - return True, 'Success' + return True, "Success" def _list_graph_waypoint_and_edge_ids(self, *args): """List the waypoint ids and edge ids of the graph currently on the robot.""" From 61c0b25bf1ca58eb785bd044263a1301c2a8b428 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 21:33:37 +0900 Subject: [PATCH 06/21] Update arg of _set_initial_localization_waypoint to string --- spot_wrapper/wrapper.py | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6395d983..6adc7b57 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1819,7 +1819,7 @@ def navigate_to( if initial_localization_fiducial: self._set_initial_localization_fiducial() if initial_localization_waypoint: - self._set_initial_localization_waypoint([initial_localization_waypoint]) + self._set_initial_localization_waypoint(initial_localization_waypoint) self._list_graph_waypoint_and_edge_ids() self._get_localization_state() resp = self._navigate_to([navigate_to]) @@ -2328,22 +2328,13 @@ def _set_initial_localization_waypoint(self, waypoint_id): """Trigger localization to a waypoint. Args: - waypoint_id (int): ID of waypoint to be localized. + waypoint_id (str): ID of waypoint to be localized. Returns: result (bool): success flag of localization message (str): message about result. """ # Take the first argument as the localization waypoint. - if type(waypoint_id) is not str: - if ( - type(waypoint_id) is list - and len(waypoint_id) > 1 - and type(waypoint_id[0]) is list - ): - waypoint_id = waypoint_id[0][0] - else: - return False, "No waypoint specified to initialize to." destination_waypoint = graph_nav_util.find_unique_waypoint_id( waypoint_id, self._current_graph, From 2ca9805037aed33d6980e4d40ce3e0c351187c7a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 20:15:19 +0900 Subject: [PATCH 07/21] Add _start_navigation() and _cancel_navigation() methods --- spot_wrapper/wrapper.py | 81 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 80 insertions(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6adc7b57..a671f995 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,3 +1,4 @@ +from distutils.dist import command_re import functools import logging import math @@ -473,7 +474,10 @@ def _start_query(self) -> None: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper.last_trajectory_command = None - self._spot_wrapper.is_moving = is_moving + if self._spot_wrapper._last_navigate_to_command != None: + is_moving = True + + self._spot_wrapper._is_moving = is_moving # We must check if any command currently has a non-None value for its id. If we don't do this, this stand # command can cause other commands to be interrupted before they get to start @@ -655,6 +659,8 @@ def __init__( self._state = RobotState() self._trajectory_status_unknown = False self._command_data = RobotCommandData() + self._last_navigate_to_command = None + self._state_navigation_valid = None self._front_image_requests = [] for source in front_image_sources: @@ -2535,6 +2541,79 @@ def _download_graph_and_snapshots( ) return True, "Success" + def _cancel_navigate_to(self): + self._navigate_to_valid = False + + @try_claim + def _start_navigation(self, target_waypoint_id): + self._lease = self._lease_wallet.get_lease() + destination_waypoint = graph_nav_util.find_unique_waypoint_id( + target_waypoint_id, + self._current_graph, + self._current_annotation_name_to_wp_id, + self._logger, + ) + + if not destination_waypoint: + self.logger.error( + "Failed to find waypoint %s in current graph.", target_waypoint_id + ) + return ( + False, + f"Failed to find waypoint {target_waypoint_id} in current graph.", + "aborted", + ) + + self.logger.info("Starting navigation to %s", destination_waypoint) + + self._lease = self._lease_wallet.advance() + sublease = self._lease.create_sublease() + self._lease_keepalive.shutdown() + + self._state_navigation_valid = True + nav_to_cmd_id = None + while self._state_navigation_valid: + time.sleep(0.5) + try: + nav_to_cmd_id = self._graph_nav_client.navigate_to( + destination_waypoint, + 1.0, + leases=[sublease.lease_proto], + command_id=nav_to_cmd_id, + ) + self._last_navigate_to_command = nav_to_cmd_id + except ResponseError as e: + self._logger.error("Error while navitation: %s", e) + break + + if self._check_success(nav_to_cmd_id): + break + + self._last_navigate_to_command = None + self._lease = self._lease_wallet.advance() + self._lease_keepalive = LeaseKeepAlive(self._lease_client) + + if self._state_navigation_valid: + return False, "Navigation is canceled", "preempted" + + status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) + if ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL + ): + return True, "Successfully completed the navigation commands!", "succeeded" + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + return False, "Robot got lost when navigating the route,", "failed" + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + return False, "Robot got stuck when navigating the route,", "failed" + elif ( + status.status + == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED + ): + return False, "Robot is impaired.", "failed" + else: + return False, "Navigation command is not complete yet.", "failed" + @try_claim def _navigate_to(self, *args): """Navigate to a specific waypoint.""" From 202e810185f7011d4d51f66d68601c0177eb262e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 20:20:56 +0900 Subject: [PATCH 08/21] Add start_navigation() and cancel_navigation() --- spot_wrapper/wrapper.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index a671f995..6db39c02 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1788,6 +1788,27 @@ def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str] except Exception as e: return False, f"Error: {e}" + def cancel_navigation(self) -> None: + """Cancel navigation of a robot from start_navigation() + """ + self._cancel_navigate_to() + + @try_claim + def start_navigation(self, target_waypoint_id: str) -> tuple(bool, str, str): + """Navigate a robot to specified waypoint id with GraphNav + + Args: + target_waypoint_id (str) : Waypont id string for where to goal + + Returns: + result (bool) : success flag + message (str) : message about result + state (str) : state of result. options are 'aborted', 'failed', 'preempted', 'succeeded'. + """ + self._get_localization_state() + resp = self._start_navigate_to(target_waypoint_id) + return resp + @try_claim def navigate_to( self, From 231e7480a73f9bd0119374eb1dc07081b83acd4f Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 24 Jun 2023 21:37:27 +0900 Subject: [PATCH 09/21] Fix format with black --- spot_wrapper/wrapper.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 6db39c02..1b27f0a1 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1789,8 +1789,7 @@ def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str] return False, f"Error: {e}" def cancel_navigation(self) -> None: - """Cancel navigation of a robot from start_navigation() - """ + """Cancel navigation of a robot from start_navigation()""" self._cancel_navigate_to() @try_claim From f3a6a196805dba0234cab92b5b1eb0845de1db15 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 29 Jun 2023 11:40:10 +0900 Subject: [PATCH 10/21] Update descriptions of localization methods --- spot_wrapper/wrapper.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 1b27f0a1..312db0d5 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1,4 +1,3 @@ -from distutils.dist import command_re import functools import logging import math @@ -1760,33 +1759,31 @@ def download_graph(self, download_path: str) -> typing.Tuple[bool, str]: ) def set_localization_fiducial(self) -> typing.Tuple[bool, str]: - """Localize a robot according to near fiducials. + """Localize a robot according to near fiducials based on current graph and snapshots in the robot. Returns: - success (bool): success flag - message (str): message about result + (bool, str) tuple indicating whether the command was successfully sent, and a message. """ try: self._set_initial_localization_fiducial() return True, "Success" except Exception as e: - return False, f"Error: {e}" + return False, f"Got an error while localizing the robot with fiducials: {e}" def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str]: - """Localize a robot to specified waypoint. + """Localize a robot to specified waypoint based on current graph and snapshots in the robot.. Args: waypoint_id (int): ID of waypoint to be localized. Returns: - result (bool): success flag of localization - message (str): message about result. + (bool, str) tuple indicating whether the command was successfully sent, and a message. """ try: resp = self._set_initial_localization_waypoint(waypoint_id) return resp[0], resp[1] except Exception as e: - return False, f"Error: {e}" + return False, f"Got an error while localizing the robot to the waypoint {waypoint_id}: {e}" def cancel_navigation(self) -> None: """Cancel navigation of a robot from start_navigation()""" From 7c9667fefc8e5c3bce54a7a97081968508b6de2a Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Thu, 29 Jun 2023 11:42:17 +0900 Subject: [PATCH 11/21] Fix type hint of start_navigation() --- spot_wrapper/wrapper.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 312db0d5..e5d9418b 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -1783,14 +1783,17 @@ def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str] resp = self._set_initial_localization_waypoint(waypoint_id) return resp[0], resp[1] except Exception as e: - return False, f"Got an error while localizing the robot to the waypoint {waypoint_id}: {e}" + return ( + False, + f"Got an error while localizing the robot to the waypoint {waypoint_id}: {e}", + ) def cancel_navigation(self) -> None: """Cancel navigation of a robot from start_navigation()""" self._cancel_navigate_to() @try_claim - def start_navigation(self, target_waypoint_id: str) -> tuple(bool, str, str): + def start_navigation(self, target_waypoint_id: str) -> typing.Tuple[bool, str, str]: """Navigate a robot to specified waypoint id with GraphNav Args: From e0bcdd115f9672083424e9b996f1e205b2c185fd Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 30 Jun 2023 09:48:42 +0900 Subject: [PATCH 12/21] Fix typo and make navigation to navigate_to --- spot_wrapper/wrapper.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index e5d9418b..9b54b3aa 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -659,7 +659,7 @@ def __init__( self._trajectory_status_unknown = False self._command_data = RobotCommandData() self._last_navigate_to_command = None - self._state_navigation_valid = None + self._state_navigate_to_valid = None self._front_image_requests = [] for source in front_image_sources: @@ -1788,12 +1788,14 @@ def set_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str] f"Got an error while localizing the robot to the waypoint {waypoint_id}: {e}", ) - def cancel_navigation(self) -> None: - """Cancel navigation of a robot from start_navigation()""" + def cancel_navigate_to(self) -> None: + """Cancel navigation of a robot from start_navigate_to()""" self._cancel_navigate_to() @try_claim - def start_navigation(self, target_waypoint_id: str) -> typing.Tuple[bool, str, str]: + def start_navigate_to( + self, target_waypoint_id: str + ) -> typing.Tuple[bool, str, str]: """Navigate a robot to specified waypoint id with GraphNav Args: @@ -2565,7 +2567,7 @@ def _cancel_navigate_to(self): self._navigate_to_valid = False @try_claim - def _start_navigation(self, target_waypoint_id): + def _start_navigate_to(self, target_waypoint_id) -> typing.Tuple[bool, str, str]: self._lease = self._lease_wallet.get_lease() destination_waypoint = graph_nav_util.find_unique_waypoint_id( target_waypoint_id, @@ -2590,9 +2592,9 @@ def _start_navigation(self, target_waypoint_id): sublease = self._lease.create_sublease() self._lease_keepalive.shutdown() - self._state_navigation_valid = True + self._state_navigate_to_valid = True nav_to_cmd_id = None - while self._state_navigation_valid: + while self._state_navigate_to_valid: time.sleep(0.5) try: nav_to_cmd_id = self._graph_nav_client.navigate_to( @@ -2613,7 +2615,7 @@ def _start_navigation(self, target_waypoint_id): self._lease = self._lease_wallet.advance() self._lease_keepalive = LeaseKeepAlive(self._lease_client) - if self._state_navigation_valid: + if self._state_navigate_to_valid: return False, "Navigation is canceled", "preempted" status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) From 104122eb1b08a1d9d9327d6c3e8b827146ca00e2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 30 Jun 2023 09:56:21 +0900 Subject: [PATCH 13/21] Fix start_navigate_to() --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 9b54b3aa..7f92f675 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2615,7 +2615,7 @@ def _start_navigate_to(self, target_waypoint_id) -> typing.Tuple[bool, str, str] self._lease = self._lease_wallet.advance() self._lease_keepalive = LeaseKeepAlive(self._lease_client) - if self._state_navigate_to_valid: + if not self._state_navigate_to_valid: return False, "Navigation is canceled", "preempted" status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id) From 4dd7fd21f4cfdf2ebd4bbfe9d88bd0e5117804d7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:12:05 +0900 Subject: [PATCH 14/21] Add typing to _cancel_navigation_to() in wrapper.py Co-authored-by: Michal Staniaszek --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 7f92f675..dceea727 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2563,7 +2563,7 @@ def _download_graph_and_snapshots( ) return True, "Success" - def _cancel_navigate_to(self): + def _cancel_navigate_to(self) -> None: self._navigate_to_valid = False @try_claim From 96f9c05bf9266a62d61bd8db249d1f4ab84ab40b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:12:25 +0900 Subject: [PATCH 15/21] Add typing to _start_navigation_to() in wrapper.py Co-authored-by: Michal Staniaszek --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index dceea727..fc5a33bc 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2567,7 +2567,7 @@ def _cancel_navigate_to(self) -> None: self._navigate_to_valid = False @try_claim - def _start_navigate_to(self, target_waypoint_id) -> typing.Tuple[bool, str, str]: + def _start_navigate_to(self, target_waypoint_id: str) -> typing.Tuple[bool, str, str]: self._lease = self._lease_wallet.get_lease() destination_waypoint = graph_nav_util.find_unique_waypoint_id( target_waypoint_id, From 8b67d0ebcb934283fc607010ae6f6fa2e1f78e36 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:16:07 +0900 Subject: [PATCH 16/21] Remove unnecessary arg and update typing for _set_initial_localization_fiducial() --- spot_wrapper/wrapper.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index fc5a33bc..5b05bdb7 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2338,7 +2338,7 @@ def _get_localization_state(self, *args): "Got robot state in kinematic odometry frame: \n%s" % str(odom_tform_body) ) - def _set_initial_localization_fiducial(self, *args): + def _set_initial_localization_fiducial(self) -> None: """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( @@ -2567,7 +2567,9 @@ def _cancel_navigate_to(self) -> None: self._navigate_to_valid = False @try_claim - def _start_navigate_to(self, target_waypoint_id: str) -> typing.Tuple[bool, str, str]: + def _start_navigate_to( + self, target_waypoint_id: str + ) -> typing.Tuple[bool, str, str]: self._lease = self._lease_wallet.get_lease() destination_waypoint = graph_nav_util.find_unique_waypoint_id( target_waypoint_id, From c2dfa5bfeb15cdd5a89bdef2b1f97eb9265912c9 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:17:31 +0900 Subject: [PATCH 17/21] Add typing to _set_initial_localization_fiducial --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 5b05bdb7..0a3b8f36 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2352,7 +2352,7 @@ def _set_initial_localization_fiducial(self) -> None: ko_tform_body=current_odom_tform_body, ) - def _set_initial_localization_waypoint(self, waypoint_id): + def _set_initial_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str]: """Trigger localization to a waypoint. Args: From 0116056a66e9032b5bcc7c38d835d02aa25f4596 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:20:10 +0900 Subject: [PATCH 18/21] Fix != None to is not None --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 0a3b8f36..e46ca593 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -473,7 +473,7 @@ def _start_query(self) -> None: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper.last_trajectory_command = None - if self._spot_wrapper._last_navigate_to_command != None: + if self._spot_wrapper._last_navigate_to_command is not None: is_moving = True self._spot_wrapper._is_moving = is_moving From 4741f5c92da742d7076a14e34bef1d98ec90912c Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:24:48 +0900 Subject: [PATCH 19/21] Fix format with black --- spot_wrapper/wrapper.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index e46ca593..81aa7bed 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2352,7 +2352,9 @@ def _set_initial_localization_fiducial(self) -> None: ko_tform_body=current_odom_tform_body, ) - def _set_initial_localization_waypoint(self, waypoint_id: str) -> typing.Tuple[bool, str]: + def _set_initial_localization_waypoint( + self, waypoint_id: str + ) -> typing.Tuple[bool, str]: """Trigger localization to a waypoint. Args: From 5bc64635605d4113f12b0caf97f21d15d9d85cc2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 1 Jul 2023 15:37:02 +0900 Subject: [PATCH 20/21] Support RobotState() and RobotCommandData() class and move _last_navigate_to_command to RobotCommandData() class --- spot_wrapper/wrapper.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 81aa7bed..a042c92e 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -473,10 +473,10 @@ def _start_query(self) -> None: self._logger.error("Error when getting robot command feedback: %s", e) self._spot_wrapper.last_trajectory_command = None - if self._spot_wrapper._last_navigate_to_command is not None: + if self._spot_wrapper.last_navigate_to_command is not None: is_moving = True - self._spot_wrapper._is_moving = is_moving + self._spot_wrapper.is_moving = is_moving # We must check if any command currently has a non-None value for its id. If we don't do this, this stand # command can cause other commands to be interrupted before they get to start @@ -593,6 +593,7 @@ class RobotCommandData: # Was the last trajectory command requested to be precise last_trajectory_command_precise: typing.Optional[bool] = None last_velocity_command_time: typing.Optional[float] = None + last_navigate_to_command: typing.Optional[int] = None class SpotWrapper: @@ -658,7 +659,6 @@ def __init__( self._state = RobotState() self._trajectory_status_unknown = False self._command_data = RobotCommandData() - self._last_navigate_to_command = None self._state_navigate_to_valid = None self._front_image_requests = [] @@ -1186,6 +1186,14 @@ def last_velocity_command_time(self) -> typing.Optional[float]: def last_velocity_command_time(self, command_time: float) -> None: self._command_data.last_velocity_command_time = command_time + @property + def last_navigate_to_command(self) -> typing.Optional[int]: + return self._command_data.last_navigate_to_command + + @last_navigate_to_command.setter + def last_navigate_to_command(self, command_id: int) -> None: + self._command_data.last_navigate_to_command = command_id + def is_estopped(self, timeout: typing.Optional[float] = None) -> bool: return self._robot.is_estopped(timeout=timeout) @@ -2607,7 +2615,7 @@ def _start_navigate_to( leases=[sublease.lease_proto], command_id=nav_to_cmd_id, ) - self._last_navigate_to_command = nav_to_cmd_id + self.last_navigate_to_command = nav_to_cmd_id except ResponseError as e: self._logger.error("Error while navitation: %s", e) break @@ -2615,7 +2623,7 @@ def _start_navigate_to( if self._check_success(nav_to_cmd_id): break - self._last_navigate_to_command = None + self.last_navigate_to_command = None self._lease = self._lease_wallet.advance() self._lease_keepalive = LeaseKeepAlive(self._lease_client) From 9bf64e4593cc9bc064fcf2b14bb0ae37825eb792 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 4 Jul 2023 15:05:21 +0900 Subject: [PATCH 21/21] Fix typo --- spot_wrapper/wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index a042c92e..52126523 100644 --- a/spot_wrapper/wrapper.py +++ b/spot_wrapper/wrapper.py @@ -2574,7 +2574,7 @@ def _download_graph_and_snapshots( return True, "Success" def _cancel_navigate_to(self) -> None: - self._navigate_to_valid = False + self._state_navigate_to_valid = False @try_claim def _start_navigate_to(