vtd: Support actuation requests from driver

Author: tobifalk

optional/vtd/src/task_control.hpp

@@ -123,12 +123,35 @@ class TaskControl : public VtdOmniSensor {
 
   using VtdOmniSensor::process;
   void process(RDB_DRIVER_CTRL_t* driver_ctrl) override {
-    steering_wheel_speed_[driver_ctrl->playerId] = driver_ctrl->steeringSpeed;
+    // Steering speed at the front wheels [rad/s].
+    if (driver_ctrl->validityFlags & RDB_DRIVER_INPUT_VALIDITY_STEERING_SPEED) {
+      steering_wheel_speed_[driver_ctrl->playerId] = driver_ctrl->steeringSpeed;
+    } else {
+      vtd_logger()->warn("{}: steeringSpeed missing in RDB_DRIVER_CTRL_t", this->get_name());
+      steering_wheel_speed_[driver_ctrl->playerId] = 0.0;
+    }
+
+    // Longitudinal acceleration request [m/s2].
+    if (driver_ctrl->validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_ACCEL) {
+      driver_request_accel_[driver_ctrl->playerId] = driver_ctrl->accelTgt;
+    } else {
+      vtd_logger()->warn("{}: accelTgt missing in RDB_DRIVER_CTRL_t", this->get_name());
+      driver_request_accel_[driver_ctrl->playerId] = 0.0;
+    }
+    // Steering request (angle at wheels) [rad].
+    if (driver_ctrl->validityFlags & RDB_DRIVER_INPUT_VALIDITY_TGT_STEERING) {
+      driver_request_steering_angle_[driver_ctrl->playerId] = driver_ctrl->steeringTgt;
+    } else {
+      vtd_logger()->warn("{}: steeringTgt missing in RDB_DRIVER_CTRL_t", this->get_name());
+      driver_request_steering_angle_[driver_ctrl->playerId] = 0.0;
+    }
   }
 
   void reset() override {
     VtdOmniSensor::reset();
     steering_wheel_speed_.clear();
+    driver_request_accel_.clear();
+    driver_request_steering_angle_.clear();
   }
 
   /**
@@ -186,10 +209,36 @@ class TaskControl : public VtdOmniSensor {
   }
 
   /**
-   * Get steering wheel speed of vehicle with given id.
+   * Get steering speed at front wheels of vehicle with given id [rad/s].
    */
   double get_steering_wheel_speed(uint64_t id) const { return steering_wheel_speed_.at(id); }
 
+  /**
+   * Get driver-requested longitudinal acceleration of vehicle with given id [m/s2].
+   */
+  double get_driver_request_acceleration(uint64_t id) const { return driver_request_accel_.at(id); }
+
+  /**
+   * Check if driver-requested longitudinal acceleration has been set.
+   */
+  bool has_driver_request_acceleration(uint64_t id) {
+    return driver_request_accel_.find(id) != driver_request_accel_.end();
+  }
+
+  /**
+   * Get driver-requested steering angle (at wheels) of vehicle with given id [rad].
+   */
+  double get_driver_request_steering_angle(uint64_t id) const {
+    return driver_request_steering_angle_.at(id);
+  }
+
+  /**
+   * Check if driver-requested steering_angle has been set.
+   */
+  bool has_driver_request_steering_angle(uint64_t id) {
+    return driver_request_steering_angle_.find(id) != driver_request_steering_angle_.end();
+  }
+
   friend void to_json(cloe::Json& j, const TaskControl& tc) {
     j = cloe::Json{{"rdb_connection", tc.rdb_}};
   }
@@ -198,6 +247,8 @@ class TaskControl : public VtdOmniSensor {
   /// RDBHandler helps us conveniently construct RDB messages.
   Framework::RDBHandler handler_;
   std::map<int, double> steering_wheel_speed_;
+  std::map<int, double> driver_request_accel_;
+  std::map<int, double> driver_request_steering_angle_;
 };
 
 }  // namespace vtd

optional/vtd/src/vtd_sensor_components.hpp

@@ -19,9 +19,10 @@
  * \file vtd_sensor_components.hpp
  */
 
-#include <cloe/component/ego_sensor.hpp>     // for EgoSensor
-#include <cloe/component/lane_sensor.hpp>    // for LaneBoundarySensor
-#include <cloe/component/object_sensor.hpp>  // for ObjectSensor
+#include <cloe/component/driver_request.hpp>  // for DriverRequest
+#include <cloe/component/ego_sensor.hpp>      // for EgoSensor
+#include <cloe/component/lane_sensor.hpp>     // for LaneBoundarySensor
+#include <cloe/component/object_sensor.hpp>   // for ObjectSensor
 
 #include "task_control.hpp"     // for TaskControl
 #include "vtd_sensor_data.hpp"  // for VtdSensorData
@@ -78,4 +79,40 @@ class VtdLaneBoundarySensor : public cloe::LaneBoundarySensor {
   std::shared_ptr<TaskControl> task_control_;
 };
 
+class VtdDriverRequest : public cloe::DriverRequest {
+ public:
+  explicit VtdDriverRequest(uint64_t id, std::shared_ptr<TaskControl> task_control)
+      : DriverRequest("vtd/driver_request"), id_(id), task_control_{task_control} {}
+
+  virtual ~VtdDriverRequest() = default;
+
+  bool has_acceleration() const override {
+    return task_control_->has_driver_request_acceleration(id_);
+  }
+
+  boost::optional<double> acceleration() const override {
+    boost::optional<double> accel;
+    if (this->has_acceleration()) {
+      accel = task_control_->get_driver_request_acceleration(id_);
+    }
+    return accel;
+  }
+
+  bool has_steering_angle() const override {
+    return task_control_->has_driver_request_steering_angle(id_);
+  }
+
+  boost::optional<double> steering_angle() const override {
+    boost::optional<double> angle;
+    if (this->has_steering_angle()) {
+      angle = task_control_->get_driver_request_steering_angle(id_);
+    }
+    return angle;
+  }
+
+ private:
+  uint64_t id_;
+  std::shared_ptr<TaskControl> task_control_;
+};
+
 }  // namespace vtd

optional/vtd/src/vtd_vehicle.hpp

@@ -84,7 +84,10 @@ class VtdVehicle : public cloe::Vehicle {
    */
   VtdVehicle(uint64_t id, const std::string& name, std::unique_ptr<RdbTransceiverTcp>&& rdb_client,
              std::shared_ptr<TaskControl> task_control)
-      : Vehicle(id, fmt::format("vtd_vehicle_{}", id)), vtd_name_(name) {
+      : Vehicle(id, fmt::format("vtd_vehicle_{}", id))
+      , vtd_name_(name)
+      , id_(id)
+      , task_control_(task_control) {
     // clang-format off
     sensor_port_ = rdb_client->tcp_port();
     vehicle_label_.tethered_to_player = name;
@@ -228,7 +231,10 @@ class VtdVehicle : public cloe::Vehicle {
       if (!sensors_.count(cfg.from)) {
         throw cloe::ModelError("reference to unknown sensor '{}'", cfg.from);
       }
-      auto sensor = this->sensors_.at(cfg.from);
+      std::shared_ptr<VtdSensorData> sensor;
+      if (cfg.from != "task_control") {
+        sensor = this->sensors_.at(cfg.from);
+      }
       auto new_or_override_component = [this](cloe::Component* c, std::string name, bool override) {
         if (override) {
           this->emplace_component(std::shared_ptr<cloe::Component>(c), name);
@@ -240,6 +246,8 @@ class VtdVehicle : public cloe::Vehicle {
         new_or_override_component(new VtdLaneBoundarySensor{sensor}, name, cfg.override);
       } else if (cfg.type == "object_sensor") {
         new_or_override_component(new VtdWorldSensor{sensor}, name, cfg.override);
+      } else if (cfg.type == "driver_request") {
+        new_or_override_component(new VtdDriverRequest{id_, task_control_}, name, cfg.override);
       } else {
         throw cloe::ModelError("unknown component type '{}'", cfg.type);
       }
@@ -250,6 +258,8 @@ class VtdVehicle : public cloe::Vehicle {
   const std::string DEFAULT_SENSOR_NAME = "cloe::vtd::sensor::default";
   std::string vtd_name_{};
   uint16_t sensor_port_{0};
+  uint16_t id_{0};
+  std::shared_ptr<TaskControl> task_control_{nullptr};
   std::map<std::string, std::shared_ptr<VtdSensorData>> sensors_;
   std::shared_ptr<VtdLatLongActuator> actuator_{nullptr};
   scp::LabelVehicle vehicle_label_;