Draft of proposed API usage for async rclrs

main.rs

fn main() {
    // BasicExecutor will be the executor that we provide out-of-the-box from rclrs.
    // Using the plain new() constructor for BasicExecutor will create a Context using
    // the user's command line arguments.
    let mut executor = rclrs::BasicExecutor::new();

    // String-like types can be automatically converted into NodeOptions.
    let node = executor.create_node("mobility");

    // Create a "worker" that manages some data that callbacks associated with this worker
    // can access and mutate. Workers cannot be associated with async callbacks, but async
    // callbacks can access the worker's data using Worker::query(). Callbacks associated
    // with this worker are strictly run one-by-one.
    let state_worker = node.create_worker(MobilityState::new());

    // Accepts an FnMut(&mut T, Msg), allowing the callback to modify the worker's
    // payload, as well as any of the closure's internal state. Any callback related
    // to a worker will be run strictly one at a time and have mutable access to the
    // worker's payload.
    let pose_update = state_worker.create_subscription(
        "pose",
        |state: &mut MobilityState, msg: geometry_msgs::msg::Pose| {
            state.pose = msg.into();
        }
    );

    let velocity_cmd = node.create_publisher::<geometry_msgs::msg::Twist>("velocity");
    let drive_timer = state_worker.create_wall_timer(
        move |state: &mut MobilityState| {
            // Periodically calculate what velocity command is needed for the
            // robot to drive towards its target.
            let twist = calculate_differential_drive_twist(&state.pose, &state.target);
            velocity_cmd.publish(twist.into());
        }
    );

    let state_worker_clone = state_worker.clone();
    let header_generator = rclrs::HeaderGenerator::new("map");
    // Accepts an Fn(Request) -> impl Future<Response>
    let planner_service = node.create_service(
        // String-like types can be automatically converted to ServiceOptions
        "planner",
        move |request| {
            async move {
                // Query the position. Await because the worker may be operating
                // on the data right now.
                let pose = state_worker_clone.query().await.pose;
                let plan = thirdpary_planner::generate_plan(pose, request.goal).await;
                nav_msgs::msg::Path {
                    header: header_generator.next(),
                    poses: plan.into(),
                }
            }
        }
    );

    // Create a client that can use the planner
    let planner_client = node.create_client("planner");

    // Create an action that carries out navigation commands
    let state_worker_clone = state_worker.clone();
    let navigate = node.create_action(
        "navigate",
        move |request, feedback| {
            async move {
                let plan = planner_client.request(request).await;
                let path = plan.path;
                let seq = plan.header.seq;
                let n_poses = path.len() as f32;
                for (i, goal_pose) in path.into_iter().enumerate() {
                    state_worker_clone.run(|state: &mut MobilityState| {
                        state.goal = goal_pose;
                        state.goal_seq = seq;
                    }).await;

                    // Each time the state_worker runs a callback that could modify
                    // its payload, this watching callback will be run. Each time that
                    // it returns None it will keep being run. The first time returns
                    // Some(T), the watcher will stop running and the future returned by
                    // .watch(~) will resolve to the T.
                    let progress = state_worker_clone.watch(|state: &MobilityState| {
                        if state.goal_seq != seq {
                            return Some(Err(state.goal_seq));
                        }
                        close_enough(state, goal_pose).then_some(Ok(state.pose))
                    }).await;

                    match progress {
                        Ok(pose) => {
                            // We reached this waypoint so send feedback on the
                            // progress that has been made.
                            feedback.send(NavigateAction_Result {
                                pose,
                                percentage: i as f32 / n_poses;
                            });
                        }
                        Err(_) => {
                            // This action was interrupted, so finish the action
                            // with a response that says it was interrupted.
                            return Navigate_Response {
                                // A different
                                result: Navigate_Response::INTERRUPTED,
                            }
                        }
                    }
                }

                // We made it through all the waypoints, so respond by saying that
                // the robot has arrived.
                Navigate_Response {
                    result: Navigate_Respons::ARRIVED,
                }
            }
        }
    );

    executor.spin();
}