Request.h

Go to the documentation of this file.

#ifndef CONTROLS_VEHICLE_REQUEST_H
#define CONTROLS_VEHICLE_REQUEST_H
 
#include <trajectory/TrajectoryBase.h>
 
#include <atomic>
#include <boost/uuid/nil_generator.hpp>
#include <boost/uuid/uuid.hpp>
#include <future>
#include <memory>
#include <optional>
 
#include "utils/NodeUtils.h"
 
namespace request {
// Request state machine:
// PENDING --> REJECTED
// PENDING --> ACTIVE --> PREEMPTED
//
// ACTIVE is the only non-terminal state besides PENDING.
// The vehicle's controller_state_ atomic tracks whether a trajectory
// is currently running or has completed.
enum class RequestHandleState { PENDING, RESOLVED };
enum class RequestState { PENDING, ACTIVE, PREEMPTED, REJECTED };
enum class RequestType { DISABLE, STATIONKEEP, TRAJECTORY };
using AtomicRequestState = std::atomic<RequestState>;
 
template <RequestHandleState S>
class RequestHandle;
 
template <RequestState S>
class Request;
 
class RequestHandleBase {
protected:
    boost::uuids::uuid id_;
    std::shared_ptr<AtomicRequestState> state_;
 
    RequestHandleBase(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state)
        : id_(id), state_(std::move(state)) {
    }
 
public:
    [[nodiscard]] boost::uuids::uuid get_id() const {
        return id_;
    }
};
 
template <>
class RequestHandle<RequestHandleState::RESOLVED> : public RequestHandleBase {
    friend class RequestHandle<RequestHandleState::PENDING>;
 
private:
    RequestHandle(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state)
        : RequestHandleBase(id, std::move(state)) {
    }
 
public:
    [[nodiscard]] bool is_active() const {
        return state_->load() == RequestState::ACTIVE;
    }
    [[nodiscard]] bool is_rejected() const {
        return state_->load() == RequestState::REJECTED;
    }
    [[nodiscard]] bool is_preempted() const {
        return state_->load() == RequestState::PREEMPTED;
    }
};
 
template <>
class RequestHandle<RequestHandleState::PENDING> : public RequestHandleBase {
    friend class Request<RequestState::PENDING>;
 
private:
    std::future<void> future_;
 
    RequestHandle(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state, std::future<void> future)
        : RequestHandleBase{id, std::move(state)}, future_(std::move(future)) {
    }
 
public:
    std::optional<RequestHandle<RequestHandleState::RESOLVED>> wait_for(std::chrono::duration<double> timeout
                                                                        = std::chrono::seconds(1)) {
        if (!future_.valid()) {
            return std::nullopt;
        }
 
        if (future_.wait_for(timeout) == std::future_status::timeout) {
            return std::nullopt;
        }
 
        try {
            future_.get();
        } catch (const std::future_error& e) {
            RCLCPP_WARN(Controller::NodeUtils::get_logger(),
                        "[REQUEST HANDLE] Future error during activation check: %s", e.what());
            return std::nullopt;
        }
 
        return RequestHandle<RequestHandleState::RESOLVED>{id_, std::move(state_)};
    }
};
 
class RequestBase {
protected:
    boost::uuids::uuid id_;
    RequestType type_;
    std::shared_ptr<AtomicRequestState> state_;
 
    RequestBase(boost::uuids::uuid id, RequestType type, std::shared_ptr<AtomicRequestState> state)
        : id_(id), type_(type), state_(std::move(state)) {
    }
 
public:
    [[nodiscard]] boost::uuids::uuid get_id() const {
        return id_;
    }
    [[nodiscard]] RequestType get_type() const {
        return type_;
    }
};
 
template <>
class Request<RequestState::ACTIVE> : public RequestBase {
public:
    static Request make_initial() {
        return {boost::uuids::nil_uuid(), RequestType::DISABLE,
                std::make_shared<AtomicRequestState>(RequestState::ACTIVE)};
    }
 
    Request& operator=(Request<RequestState::ACTIVE>&& other) noexcept {
        if (this == &other) {
            return *this;
        }
        state_->store(RequestState::PREEMPTED);
        id_    = other.id_;
        type_  = other.type_;
        state_ = std::move(other.state_);
        return *this;
    }
 
    ~Request()                                        = default;
    Request(Request<RequestState::ACTIVE>&&) noexcept = default;
    Request(const Request&)                           = delete;
    Request& operator=(const Request&)                = delete;
 
private:
    friend class Request<RequestState::PENDING>;
    Request(boost::uuids::uuid id, RequestType type, std::shared_ptr<AtomicRequestState> state)
        : RequestBase{id, type, std::move(state)} {
    }
};
 
template <>
class Request<RequestState::PENDING> : public RequestBase {
public:
    Request(boost::uuids::uuid id, RequestType type, std::unique_ptr<TrajectoryBase> trajectory = nullptr)
        : RequestBase{id, type, std::make_shared<AtomicRequestState>(RequestState::PENDING)},
          trajectory_(std::move(trajectory)) {
    }
 
    [[nodiscard]] RequestHandle<RequestHandleState::PENDING> get_handle() {
        return {id_, state_, promise_.get_future()};
    }
 
    std::pair<Request<RequestState::ACTIVE>, std::unique_ptr<TrajectoryBase>> accept() {
        state_->store(RequestState::ACTIVE);
        promise_.set_value();
        Request<RequestState::ACTIVE> active(id_, type_, std::move(state_));
        return {std::move(active), std::move(trajectory_)};
    }
 
    void reject() {
        state_->store(RequestState::REJECTED);
        promise_.set_value();
    }
 
private:
    std::unique_ptr<TrajectoryBase> trajectory_;
    std::promise<void> promise_;
};
 
}  // namespace request
 
#endif  // CONTROLS_VEHICLE_REQUEST_H