Navigate to staging pose cannot be terminated

[redvinaa]

Terminate during navigation is not handled.
I think instead of the spin_until_future_complete here, there should be a loop that checks if the action was terminated (which has to be propagated from above).

[SteveMacenski]

This seems correct to me. Please be more specific.

  auto future_goal_handle = nav_to_pose_client_->async_send_goal(goal);  // Note: Sends goal
  if (executor_.spin_until_future_complete(
      future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS)
  {
    auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get());
    if (executor_.spin_until_future_complete(
        future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS)  // Note: Spins until completed, by any means
    {
      auto result = future_result.get();
      if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) {  // Note: if we got to the pose, we're successful and can exit
        return;  // Success!
      }
    }
  }

  // Attempt to retry once using single iteration recursion
  if (!recursed) {
    goToPose(pose, max_staging_duration, true);  // Note: Else, if we're not successful, goal didn't send, or terminated action, attempt again
    return;
  }

  throw opennav_docking_core::FailedToStage("Navigation request to staging pose failed.");  // Note: Or, throw exception and fail action
}

[redvinaa]

I'm sorry, let me explain.
When the navigation tree fails, the error propagates up correctly to where DockRobot was called from. But in the other direction it doesn't, so if you cancel the dock action by hand during navigation to staging area, the robot will finish navigating there, and then stops.

Later at the precise docking stage this is handled correctly in a loop:

    while (rclcpp::ok()) {
      try {
        // Approach the dock using control law
        if (approachDock(dock, dock_pose)) {
          ...
        }

        // Cancelled, preempted, or shutting down (recoverable errors throw DockingException)
        stashDockData(goal->use_dock_id, dock, false);
        publishZeroVelocity();
        docking_action_server_->terminate_all(result);

And in approachDock we check for cancellation.

    // Stop if cancelled/preempted
    if (checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
      checkAndWarnIfPreempted(docking_action_server_, "dock_robot"))
    {
      return false;
    }

But above for the navigation part we don't check so it doesn't terminate:

      navigator_->goToPose(
        initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
      RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");

[SteveMacenski]

Ohhh, ok. That is not what I thought you were going with this. That seems like a great idea then

We can have a while loop for spinning the executor for a 1s timeout, check if cancellation is requested, and keep looping up to the max_staging_duration duration. If we find that a cancel was asked for, then we send the cancel request to the navigation action.

The only API change that should be required is passing goToPose in a lambda or std::function that wraps action_server->is_preemption_requested() so that the loop can know the state of the docking action for the above logic

Sound good?

[redvinaa]

All right. I wrote the code, I'm gonna open the PR as soon as I've had the time to test.