Line data    Source code

       1             : #ifndef CONTROLS_VEHICLE_REQUEST_H
       2             : #define CONTROLS_VEHICLE_REQUEST_H
       3             : 
       4             : #include <trajectory/TrajectoryBase.h>
       5             : 
       6             : #include <atomic>
       7             : #include <boost/uuid/nil_generator.hpp>
       8             : #include <boost/uuid/uuid.hpp>
       9             : #include <future>
      10             : #include <memory>
      11             : #include <optional>
      12             : 
      13             : #include "utils/NodeUtils.h"
      14             : 
      15             : namespace request {
      16             : // Request state machine:
      17             : // PENDING --> REJECTED
      18             : // PENDING --> ACTIVE --> PREEMPTED
      19             : //
      20             : // ACTIVE is the only non-terminal state besides PENDING.
      21             : // The vehicle's controller_state_ atomic tracks whether a trajectory
      22             : // is currently running or has completed.
      23           0 : enum class RequestHandleState { PENDING, RESOLVED };
      24           0 : enum class RequestState { PENDING, ACTIVE, PREEMPTED, REJECTED };
      25           0 : enum class RequestType { DISABLE, STATIONKEEP, TRAJECTORY };
      26           0 : using AtomicRequestState = std::atomic<RequestState>;
      27             : 
      28             : template <RequestHandleState S>
      29           0 : class RequestHandle;
      30             : 
      31             : template <RequestState S>
      32           0 : class Request;
      33             : 
      34           0 : class RequestHandleBase {
      35             : protected:
      36             :     boost::uuids::uuid id_;
      37             :     std::shared_ptr<AtomicRequestState> state_;
      38             : 
      39           0 :     RequestHandleBase(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state)
      40             :         : id_(id), state_(std::move(state)) {
      41             :     }
      42             : 
      43             : public:
      44           0 :     [[nodiscard]] boost::uuids::uuid get_id() const {
      45             :         return id_;
      46             :     }
      47             : };
      48             : 
      49             : template <>
      50           0 : class RequestHandle<RequestHandleState::RESOLVED> : public RequestHandleBase {
      51             :     friend class RequestHandle<RequestHandleState::PENDING>;
      52             : 
      53             : private:
      54             :     RequestHandle(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state)
      55             :         : RequestHandleBase(id, std::move(state)) {
      56             :     }
      57             : 
      58             : public:
      59           0 :     [[nodiscard]] bool is_active() const {
      60             :         return state_->load() == RequestState::ACTIVE;
      61             :     }
      62           0 :     [[nodiscard]] bool is_rejected() const {
      63             :         return state_->load() == RequestState::REJECTED;
      64             :     }
      65           0 :     [[nodiscard]] bool is_preempted() const {
      66             :         return state_->load() == RequestState::PREEMPTED;
      67             :     }
      68             : };
      69             : 
      70             : template <>
      71           0 : class RequestHandle<RequestHandleState::PENDING> : public RequestHandleBase {
      72             :     friend class Request<RequestState::PENDING>;
      73             : 
      74             : private:
      75             :     std::future<void> future_;
      76             : 
      77             :     RequestHandle(boost::uuids::uuid id, std::shared_ptr<AtomicRequestState> state, std::future<void> future)
      78             :         : RequestHandleBase{id, std::move(state)}, future_(std::move(future)) {
      79             :     }
      80             : 
      81             : public:
      82           0 :     std::optional<RequestHandle<RequestHandleState::RESOLVED>> wait_for(std::chrono::duration<double> timeout
      83             :                                                                         = std::chrono::seconds(1)) {
      84             :         if (!future_.valid()) {
      85             :             return std::nullopt;
      86             :         }
      87             : 
      88             :         if (future_.wait_for(timeout) == std::future_status::timeout) {
      89             :             return std::nullopt;
      90             :         }
      91             : 
      92             :         try {
      93             :             future_.get();
      94             :         } catch (const std::future_error& e) {
      95             :             RCLCPP_WARN(Controller::NodeUtils::get_logger(),
      96             :                         "[REQUEST HANDLE] Future error during activation check: %s", e.what());
      97             :             return std::nullopt;
      98             :         }
      99             : 
     100             :         return RequestHandle<RequestHandleState::RESOLVED>{id_, std::move(state_)};
     101             :     }
     102             : };
     103             : 
     104           0 : class RequestBase {
     105             : protected:
     106             :     boost::uuids::uuid id_;
     107             :     RequestType type_;
     108             :     std::shared_ptr<AtomicRequestState> state_;
     109             : 
     110           0 :     RequestBase(boost::uuids::uuid id, RequestType type, std::shared_ptr<AtomicRequestState> state)
     111             :         : id_(id), type_(type), state_(std::move(state)) {
     112             :     }
     113             : 
     114             : public:
     115           0 :     [[nodiscard]] boost::uuids::uuid get_id() const {
     116             :         return id_;
     117             :     }
     118           0 :     [[nodiscard]] RequestType get_type() const {
     119             :         return type_;
     120             :     }
     121             : };
     122             : 
     123             : template <>
     124           0 : class Request<RequestState::ACTIVE> : public RequestBase {
     125             : public:
     126           0 :     static Request make_initial() {
     127             :         return {boost::uuids::nil_uuid(), RequestType::DISABLE,
     128             :                 std::make_shared<AtomicRequestState>(RequestState::ACTIVE)};
     129             :     }
     130             : 
     131           0 :     Request& operator=(Request<RequestState::ACTIVE>&& other) noexcept {
     132             :         if (this == &other) {
     133             :             return *this;
     134             :         }
     135             :         state_->store(RequestState::PREEMPTED);
     136             :         id_    = other.id_;
     137             :         type_  = other.type_;
     138             :         state_ = std::move(other.state_);
     139             :         return *this;
     140             :     }
     141             : 
     142           0 :     ~Request()                                        = default;
     143           0 :     Request(Request<RequestState::ACTIVE>&&) noexcept = default;
     144           0 :     Request(const Request&)                           = delete;
     145           0 :     Request& operator=(const Request&)                = delete;
     146             : 
     147             : private:
     148             :     friend class Request<RequestState::PENDING>;
     149             :     Request(boost::uuids::uuid id, RequestType type, std::shared_ptr<AtomicRequestState> state)
     150             :         : RequestBase{id, type, std::move(state)} {
     151             :     }
     152             : };
     153             : 
     154             : template <>
     155           0 : class Request<RequestState::PENDING> : public RequestBase {
     156             : public:
     157           0 :     Request(boost::uuids::uuid id, RequestType type, std::unique_ptr<TrajectoryBase> trajectory = nullptr)
     158             :         : RequestBase{id, type, std::make_shared<AtomicRequestState>(RequestState::PENDING)},
     159             :           trajectory_(std::move(trajectory)) {
     160             :     }
     161             : 
     162           0 :     [[nodiscard]] RequestHandle<RequestHandleState::PENDING> get_handle() {
     163             :         return {id_, state_, promise_.get_future()};
     164             :     }
     165             : 
     166           0 :     std::pair<Request<RequestState::ACTIVE>, std::unique_ptr<TrajectoryBase>> accept() {
     167             :         state_->store(RequestState::ACTIVE);
     168             :         promise_.set_value();
     169             :         Request<RequestState::ACTIVE> active(id_, type_, std::move(state_));
     170             :         return {std::move(active), std::move(trajectory_)};
     171             :     }
     172             : 
     173           0 :     void reject() {
     174             :         state_->store(RequestState::REJECTED);
     175             :         promise_.set_value();
     176             :     }
     177             : 
     178             : private:
     179             :     std::unique_ptr<TrajectoryBase> trajectory_;
     180             :     std::promise<void> promise_;
     181             : };
     182             : 
     183             : }  // namespace request
     184             : 
     185             : #endif  // CONTROLS_VEHICLE_REQUEST_H