Deferrable + Cancelable lifecycle change_state transition function implementation

[tgroechel]

Addresses: #2213
Depends on: ros2/rcl_interfaces#157
Demo Code

Lifecycle re-architecture dependency: #2212 / #2211

This is a proposed first-stage solution:

• maintains backward compatibility (the core reason for not directly converting to a ROS 2 Action)
• attempts to structure the code to allow for possible future transition to using a ROS 2 Action for ChangeState

Example Usage

Note code is collapsed / expandable so this post is more readable

1. Simple usage by spawning a thread, note the ChangeStateHandler is similar to a GoalHandle
2. Simple parameter request that works a lot like a coroutine ("throws" a future callback onto the executor thus it is the executors job to receive / call the future callback)
3. Cancel example with polling. Demo Code, Demo video
4. BulkRequester: a wrapper to help with a set of async request (e.g., parameter requests) that can monitor for cancels.

Expand example 1 code

void on_activate_async(
     const rclcpp_lifecycle::State &state,
     std::shared_ptr<rclcpp_lifecycle::ChangeStateHandler> change_state_hdl) {
   std::thread t(&LifecycleTalker::defer_on_activate_work, this,change_state_hdl);
   t.detach();
} // Off Executor

void defer_on_activate_work(
   std::shared_ptr<rclcpp_lifecycle::ChangeStateHandler> change_state_hdl) {
   /*DO WORK*/
   LifecycleNode::on_activate(state); // activate ManagedEntities
   change_state_hdl->send_callback_resp(CallbackReturn::SUCCESS);
}

Expand example 2 code

void on_configure_async(
   const rclcpp_lifecycle::State &,
   std::shared_ptr<rclcpp_lifecycle::ChangeStateHandler> change_state_hdl) {
      
   // Callback for future response of getting a parameter
   auto response_received_callback =
      [logger = this->get_logger(), change_state_hdl](ParamSharedFuture future) {
         if (change_state_hdl->is_executing()) {
           m_param1 = future.get();
           change_state_hdl->send_callback_resp(CallbackReturn::SUCCESS);
         }
      };

   // Sending the request and attaching the callback
   auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
   request->names.push_back("param1");
   client_->async_send_request(request, std::move(response_received_callback));
} // Off Executor

Expand example 3 code

void on_configure_async(
   const rclcpp_lifecycle::State &,
   std::shared_ptr<rclcpp_lifecycle::ChangeStateHandler> change_state_hdl) {
   // Cancel monitoring
   transition_cancel_monitoring_timer_ = create_wall_timer(
      std::chrono::milliseconds{100}, [this, change_state_hdl]() {
         if (!change_state_hdl->is_executing()) {
           transition_cancel_monitoring_timer_.reset();
           return;
         } else if (change_state_hdl->is_canceling()) { /*handle cancel*/
           size_t num_pruned_req = client_->prune_pending_requests();
           change_state_hdl->handle_canceled(true);
           transition_cancel_monitoring_timer_.reset();
         }
      });

   // Callback for future response of getting a parameter
   auto response_received_callback =
      [logger = this->get_logger(), change_state_hdl](ParamSharedFuture future) {
         if (change_state_hdl->is_executing()) {
           m_param1 = future.get();
           change_state_hdl->send_callback_resp(CallbackReturn::SUCCESS);
         }
      };

   // Sending the request and attaching the callback
   auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
   request->names.push_back("param1");
   client_->async_send_request(request, std::move(response_received_callback));
} // Off executor

Expand example 4 code

void on_configure_async(
   const rclcpp_lifecycle::State &,
   std::shared_ptr<rclcpp_lifecycle::ChangeStateHandler> change_state_hdl){

   auto configure_param_req_done_cb = 
      [this, change_state_hdl](vector<AsyncReq> failed_requests){
         if(failed_requests.empty()){
           change_state_hdl->send_callback_resp(CallbackReturn::SUCCESS);
         }
         else if(change_state_hdl->is_cancelling()){
         /*Clean up failed requests*/
           change_state_hdl->handled_cancel(true);
         }
         else {
         /*Clean up failed requests*/
           change_state_hdl->send_callback_resp(CallbackReturn::FAILURE);
         }
      };
   m_bulk_async_requester.register_change_state_hdl(change_state_hdl);
   m_bulk_async_requester.register_completed_cb(std::move(configure_param_req_done_cb));
   m_bulk_async_requester.add_param_req("t0/turtle_node", "use_sim_time", m_t0_uses_sim_time);
   m_bulk_async_requester.add_param_req("t1/turtle_node", "use_sim_time", m_t1_uses_sim_time);
   /*...*/
   m_bulk_async_requester.send_requests();
}

Additional details found within expandable sections below

Expand

ChangeStateHandler

An async transition follows the same flow as before but now passes a ChangeState handler that:

• allows response deferral
• allows for handling a cancel request

This is very similar to a GoalHandler within a ROS 2 Action. However, we want to maintain backward compatibility.

Async transition

Note the expansion blocks for images

Control flow of an async transition

The above image outlines the process. Only 1 request can be processed at a given time, all other requests are rejected. The ChangeStateHandler allows for the user to send a response whenever they are done with the transition (accomplished by passing a newly created shared_ptr<ChangeStateHandler> to the user).

Cancelling a transition

Reference for `Callback::FAILURE` paths of lifecycle nodes

(Note RAISE_ERROR is not currently merged having an open set of issues described here: ros2/design#283 (comment))

Given that the transition is async, it would be ideal if it could also have the potential to be cancelled. The goal would to attempt to recover the node into a valid primary state. With this:

• only the user can handle a cancel safely so the ChangeStateHandler is "marked" as cancelled and waits until the user marks the cancel as completed
• if handled successfully, the state follows a CallbackReturn::Failure path. This is desired to
  • recover, mostly, to the last primary state (see reference diagram above)
  • maintain state_machine validity/backward compatibility
  • reuse the same code as change_state

Control flow of an async transition

The example flow of a cancelled transition. Note this is all possible even on a SingleThreadedExecutor as the timer acts as a monitor for the transition. Note the rejection and failure criteria / reasons are passed back to the client within the CancelTransition.srv.

<\details>

[ros-discourse]

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/deferrable-canceleable-lifecycle-transitions/32318/1