`/navigate_to` fails with `LeaseNotOwnedByWallet: Status.OTHER_OWNER` (and sometimes `RobotImpairedError`)

[chiar15]

Hi! I’m having trouble using the /navigate_to action.

Before sending the goal, I clear the graph, upload the map, and set the localization method using the corresponding services. All of these steps complete without any errors. I also acquire the lease beforehand, and by checking the /status/leases topic I can confirm that my node has the lease for all resources (including body).

However, every time I send a /navigate_to goal, the action fails with a lease error saying that the body lease is owned by someone else:

[spot_ros2-1] [ERROR] [1770292318.048565582] [spot_ros2]: Error raised in execute callback: Lease on "body" has state (Status.OTHER_OWNER) [spot_ros2-1] Traceback (most recent call last): [spot_ros2-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/action/server.py", line 336, in _execute_goal [spot_ros2-1] execute_result = await await_or_execute(execute_callback, goal_handle) [spot_ros2-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute [spot_ros2-1] return callback(*args) [spot_ros2-1] File "/home/chiara/ros2_ws/install/spot_driver/lib/spot_driver/spot_ros2", line 3017, in handle_navigate_to [spot_ros2-1] resp = self.spot_wrapper.spot_graph_nav._navigate_to( [spot_ros2-1] File "/home/chiara/ros2_ws/build/spot_wrapper/spot_wrapper/wrapper_helpers.py", line 55, in wrapper_try_claim [spot_ros2-1] return func(*args, **kwargs) [spot_ros2-1] File "/home/chiara/ros2_ws/build/spot_wrapper/spot_wrapper/spot_graph_nav.py", line 517, in _navigate_to [spot_ros2-1] self._lease = self._lease_wallet.advance() [spot_ros2-1] File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 421, in advance [spot_ros2-1] lease_state = self._get_owned_lease_state_locked(resource) [spot_ros2-1] File "/usr/local/lib/python3.10/dist-packages/bosdyn/client/lease.py", line 483, in _get_owned_lease_state_locked [spot_ros2-1] raise LeaseNotOwnedByWallet(resource, lease_state) [spot_ros2-1] bosdyn.client.lease.LeaseNotOwnedByWallet: Lease on "body" has state (Status.OTHER_OWNER) [spot_ros2-1] [WARN] [1770292318.049514152] [spot_ros2]: Goal state not set, assuming aborted. Goal ID: [ 70 20 32 40 242 3 67 19 181 210 228 59 27 177 238 184] [spot_ros2-1] [ERROR] [1770292348.392360039] [spot_ros2]: Error raised in execute callback: bosdyn.api.graph_nav.NavigateToResponse (RobotImpairedError): Robot has a critical perception or behavior fault and cannot navigate.

What confuses me is that /status/leases shows that I do own the leases, yet the navigation code fails when it tries to advance the lease (it looks like lease_wallet.advance() raises because the wallet thinks it doesn’t own the body lease).

The only workaround I’ve found is to explicitly release the claim before sending the goal, and then re-acquire it after the goal finishes (success or failure). This “works”, but it’s quite inconvenient and creates problems in my workflow.

Am I missing something in the correct lease workflow for /navigate_to? Is there a recommended way to avoid this Status.OTHER_OWNER error, or to properly synchronize the lease used by GraphNav with the one reported on /status/leases?

[tcappellari-bdai]

Are you running everything on the same machine?

[chiar15]

Yes

[mhidalgo-rai]

@chiar15 would you mind collecting some stacktraces for us? In particular, it would be super useful if you could patch your local bosdyn SDK here and get the contents of lease_state and lease_use_result. Also traceback.print_stack() to get a grasp of what (likely async) gRPC call is causing this.

[chiar15]

Sure, I'll patch it and get you the stacktraces. Quick note though, the robot is at my university and I can only use it outdoors. We're supposed to get rain over the next few days, so I'm not sure when I'll be able to test this. I'll post the info as soon as I can.

[chiar15]

debug_spot.txt

I've added the debug code and attached the full output that I got as a result.

[mhidalgo-rai]

Hmm, it looks like the GraphNav system on Spot took the lease and advanced it from (4, 2) to (4, 4, 1, 1) without Spot ROS 2 knowing, crashing it when trying to advance the lease:

[spot_ros2-1] attempted_lease {
[spot_ros2-1]   resource: "body"
[spot_ros2-1]   epoch: "aBUdjmzxYrsvTpbD"
[spot_ros2-1]   sequence: 4
[spot_ros2-1]   sequence: 2
[spot_ros2-1]   client_names: "root"
[spot_ros2-1]   client_names: "ros_spot"
[spot_ros2-1] }
[spot_ros2-1] previous_lease {
[spot_ros2-1]   resource: "body"
[spot_ros2-1]   epoch: "aBUdjmzxYrsvTpbD"
[spot_ros2-1]   sequence: 4
[spot_ros2-1]   sequence: 4
[spot_ros2-1]   sequence: 1
[spot_ros2-1]   sequence: 1
[spot_ros2-1]   client_names: "root"
[spot_ros2-1]   client_names: "ros_spot"
[spot_ros2-1]   client_names: "bosdyn.graph_nav"
[spot_ros2-1]   client_names: "internal.localnav"
[spot_ros2-1] }
[spot_ros2-1] latest_known_lease {
[spot_ros2-1]   resource: "body"
[spot_ros2-1]   epoch: "aBUdjmzxYrsvTpbD"
[spot_ros2-1]   sequence: 4
[spot_ros2-1]   sequence: 4
[spot_ros2-1]   sequence: 1
[spot_ros2-1]   sequence: 1
[spot_ros2-1]   client_names: "root"
[spot_ros2-1]   client_names: "ros_spot"
[spot_ros2-1]   client_names: "bosdyn.graph_nav"
[spot_ros2-1]   client_names: "internal.localnav"
[spot_ros2-1] }

This code has not seen an update in a long while, and I suspect we are not subleasing consistently. IIUC the lease Spot ROS 2 holds should always be a "parent" lease of the one GraphNav gets i.e. (4, 2, ...) in this case.

[chiar15]

Got it, thanks for clarifying. Should I wait for a fix in the driver, or is there something I can adjust in my setup for now? As I said I was working around it by releasing and re-acquiring the lease after each navigation, but every time Spot sits down and I have to wait until it stands again, so it's not really ideal.

[tcappellari-bdai]

PRs are always welcome if you have a fix!
But we are now aware of the issue and have added it to our backlog. Unfortunately, I can't guarantee when we will have the bandwidth to get to it though.

[chiar15]

I actually managed to solve the problem by modifying the following method inside spot_graph_nav.py which is inside the package spot_wrapper:

def _navigate_to(
        self,
        waypoint_id: str
    ) -> typing.Tuple[bool, str]:
        """Navigate to a specific waypoint."""
        destination_waypoint = self._find_unique_waypoint_id(
            waypoint_id,
            self._current_graph,
            self._current_annotation_name_to_wp_id,
            self._logger,
        )
        if not destination_waypoint:
            self._logger.error("Failed to find the appropriate unique waypoint id for the navigation command.")
            return (
                False,
                "Failed to find the appropriate unique waypoint id for the navigation command.",
            )

        self._lease = self._lease_wallet.get_lease()

        # Navigate to the destination waypoint.
        is_finished = False
        nav_to_cmd_id = -1

        while not is_finished and rclpy.ok():

            # Issue the navigation command about twice a second such that it is easy to terminate the
            # navigation command (with estop or killing the program).
            nav_to_cmd_id = self._graph_nav_client.navigate_to(
                destination_waypoint, 1.0, leases=[self._lease.lease_proto]
            )
            time.sleep(0.5)  # Sleep for half a second to allow for command execution.
            # Poll the robot for feedback to determine if the navigation command is complete.
            is_finished = self._check_success(nav_to_cmd_id)

        self._lease = self._lease_wallet.advance()

        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!"
        elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
            return (
                False,
                "Robot got lost when navigating the route, the robot will now sit down.",
            )
        elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
            return (
                False,
                "Robot got stuck when navigating the route, the robot will now sit down.",
            )
        elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
            return False, "Robot is impaired."
        else:
            return False, "Navigation command is not complete yet."

I honestly don't whether this is the right way to use leases or not, but it's what worked for me

[tcappellari-bdai]

Great! Could you open a PR with that fix?