vtd: Add external ego model class

Author: tobifalk

optional/vtd/CMakeLists.txt

@@ -62,6 +62,7 @@ if(BUILD_TESTING)
         src/rdb_transceiver_tcp_test.cpp
         src/osi_test.cpp
         src/vtd_osi_test.cpp
+        src/vtd_data_conversion_test.cpp
     )
     set_target_properties(test-vtd-binding PROPERTIES
         CXX_STANDARD 14

optional/vtd/src/actuator_component.hpp

@@ -26,7 +26,8 @@
 
 #include <memory>  // for shared_ptr<>
 
-#include <cloe/component/latlong_actuator.hpp>  // for LatLongActuator
+#include <cloe/component/latlong_actuator.hpp>     // for LatLongActuator
+#include <cloe/component/vehicle_state_model.hpp>  // for VehicleStateModel
 
 #include "task_control.hpp"  // for TaskControl
 #include "vtd_logger.hpp"    // for vtd_logger
@@ -157,21 +158,73 @@ class VtdLatLongActuator : public VtdVehicleControl, public cloe::LatLongActuato
   cloe::utility::ActuationLevel old_level_;
 };
 
+/**
+ * VtdExternalEgoModel provides the ego state from an external model to VTD.
+ *
+ * # Usage
+ *
+ * Every VTD cycle, the following needs to be done:
+ *
+ * - `step_begin` retrieves the updated ego vehicle state from the external model
+ *    and registers the new state with the TaskControl client.
+ * - `TaskControl::add_trigger_and_send` must be called to send the information
+ *   to VTD.
+ */
+class VtdExternalEgoModel : public VtdVehicleControl, public cloe::VehicleStateModel {
+ public:
+  VtdExternalEgoModel(std::shared_ptr<TaskControl> tc, uint64_t id, const std::string& veh_name)
+      : VehicleStateModel("vtd/ego_state")
+      , task_control_(tc)
+      , vehicle_id_(id)
+      , vehicle_name_(veh_name) {}
+  virtual ~VtdExternalEgoModel() = default;
+
+  /**
+   * Add the DynObjectState package to the TaskControl.
+   *
+   * This should only be called once per simulation step. This method will not
+   * pay attention for you.  Later, when the TaskControl sends its packages,
+   * this one will be part of it.
+   */
+  void step_begin(const cloe::Sync& sync) override {
+    if (this->is_vehicle_state()) {
+      add_dyn_object_state();
+    } else if (sync.step() > 1) {
+      // During the first time step, external model has not yet been updated. Throw otherwise.
+      throw cloe::ModelError("VtdExternalEgoModel: vehicle state not set.");
     }
   }
 
-  cloe::Duration process(const cloe::Sync& sync) override { return LatLongActuator::process(sync); }
-
   void reset() override {
-    old_level_.set_none();
-    LatLongActuator::reset();
+    VehicleStateModel::reset();
     task_control_->reset();
   }
 
+  cloe::Json to_json() const override {
+    return dynamic_cast<const cloe::VehicleStateModel&>(*this);
+  }
+
+ private:
+  void add_dyn_object_state() {
+    auto ego_state = this->vehicle_state();
+    assert(ego_state);
+    DynObjectState os;
+    assert(static_cast<uint64_t>(ego_state->id) == vehicle_id_);
+    os.base_id = ego_state->id;
+    os.base_type = cloe_vtd_obj_class_map.at(ego_state->classification);
+    os.base_name = vehicle_name_;
+    os.base_geo = rdb_geometry_from_object(ego_state.get());
+    os.base_pos = rdb_coord_from_object(ego_state.get());
+    os.ext_speed = rdb_coord_from_vector3d(ego_state->velocity, ego_state->angular_velocity);
+    os.ext_accel = rdb_coord_pos_from_vector3d(ego_state->acceleration);
+    // Add new ego state to task control message.
+    task_control_->add_dyn_object_state(os);
+  }
+
  private:
   std::shared_ptr<TaskControl> task_control_;
   uint64_t vehicle_id_;
-  cloe::utility::ActuationLevel old_level_;
+  std::string vehicle_name_;
 };
 
 }  // namespace vtd

optional/vtd/src/task_control.hpp

@@ -31,7 +31,8 @@
   #include <VtdToolkit/RDBHandler.hh>
 #endif  
 
-#include <cloe/core.hpp>  // for Json
+#include <cloe/component/object.hpp>  // for Object
+#include <cloe/core.hpp>              // for Json
 
 #include "omni_sensor_component.hpp"  // for VtdOmniSensor
 #include "rdb_codec.hpp"              // for RdbCodec
@@ -77,6 +78,97 @@ struct DriverControl {
   }
 };
 
+struct DynObjectState {
+  /// Object id.
+  uint32_t base_id{0};
+
+  /// Object category (player, sensor, ...).
+  uint8_t base_category{RDB_OBJECT_CATEGORY_PLAYER};
+
+  /// Object type (car, truck, ...).
+  uint8_t base_type{RDB_OBJECT_TYPE_NONE};
+
+  /// Visibility mask (e.g. visible for traffic and visible for data recorder).
+  uint16_t base_vis_mask{RDB_OBJECT_VIS_FLAG_TRAFFIC | RDB_OBJECT_VIS_FLAG_RECORDER};
+
+  /// Player name.
+  std::string base_name;
+
+  /// Object dimension and offset to cog.
+  RDB_GEOMETRY_t base_geo;
+
+  /// Object position and orientation.
+  RDB_COORD_t base_pos;
+
+  /// Object velocity and angular velocity.
+  RDB_COORD_t ext_speed;
+
+  /// Object acceleration and angular acceleration.
+  RDB_COORD_t ext_accel;
+
+  friend void to_json(cloe::Json& j, const DynObjectState& os) {
+    j = cloe::Json{
+        {"base_id", os.base_id},     {"base_category", os.base_category},
+        {"base_type", os.base_type}, {"base_vis_mask", os.base_vis_mask},
+        {"base_name", os.base_name},
+    };
+  }
+};
+
+/**
+ * Map to convert from Cloe to VTD object classification.
+ */
+const std::map<cloe::Object::Class, uint8_t> cloe_vtd_obj_class_map = {
+    {cloe::Object::Class::Car, RDB_OBJECT_TYPE_PLAYER_CAR},
+    {cloe::Object::Class::Truck, RDB_OBJECT_TYPE_PLAYER_TRUCK},
+    {cloe::Object::Class::Motorbike, RDB_OBJECT_TYPE_PLAYER_MOTORBIKE},
+    {cloe::Object::Class::Trailer, RDB_OBJECT_TYPE_PLAYER_TRAILER},
+};
+
+/**
+ * Convert object geometry VTD geometry.
+ */
+RDB_GEOMETRY_t rdb_geometry_from_object(const cloe::Object& obj) {
+  RDB_GEOMETRY_t geo;
+  geo.dimX = obj.dimensions.x();
+  geo.dimY = obj.dimensions.y();
+  geo.dimZ = obj.dimensions.z();
+  geo.offX = obj.cog_offset.x();
+  geo.offY = obj.cog_offset.y();
+  geo.offZ = obj.cog_offset.z();
+  return geo;
+}
+
+RDB_COORD_t rdb_coord_from_vector3d(const Eigen::Vector3d& position,
+                                    const Eigen::Vector3d& angle_rph) {
+  RDB_COORD_t coord;
+  coord.x = position.x();
+  coord.y = position.y();
+  coord.z = position.z();
+  coord.r = angle_rph.x();
+  coord.p = angle_rph.y();
+  coord.h = angle_rph.z();
+  coord.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
+  coord.type = RDB_COORD_TYPE_INERTIAL;
+  return coord;
+}
+
+RDB_COORD_t rdb_coord_from_object(const cloe::Object& obj) {
+  Eigen::Vector3d hpr = obj.pose.rotation().matrix().eulerAngles(2, 1, 0);
+  return rdb_coord_from_vector3d(obj.pose.translation(),
+                                 Eigen::Vector3d(hpr.z(), hpr.y(), hpr.x()));
+}
+
+RDB_COORD_t rdb_coord_pos_from_vector3d(const Eigen::Vector3d& position) {
+  RDB_COORD_t coord;
+  coord.x = position.x();
+  coord.y = position.y();
+  coord.z = position.z();
+  coord.flags = RDB_COORD_FLAG_POINT_VALID;
+  coord.type = RDB_COORD_TYPE_INERTIAL;
+  return coord;
+}
+
 /**
  * TaskControl contains the connection to the VTD task control server.
  *
@@ -172,6 +264,29 @@ class TaskControl : public VtdOmniSensor {
     driverCtrl->validityFlags = dc.validity_flags;
   }
 
+  void add_dyn_object_state(const DynObjectState& os) {
+    // TODO(tobias): From the implementation of `add_driver_control`, it seems
+    // that the actual sim time and frame no. are not needed..
+    handler_.addPackage(0.0, 0, RDB_PKG_ID_START_OF_FRAME);
+    RDB_OBJECT_STATE_t* objState = reinterpret_cast<RDB_OBJECT_STATE_t*>(
+        handler_.addPackage(0.0, 0, RDB_PKG_ID_OBJECT_STATE, /*noElements=*/1,
+                            /*extended=*/true));
+    if (objState == nullptr) {
+      vtd_logger()->error("TaskControl: cannot add RDB_OBJECT_STATE package");
+      return;
+    }
+    objState->base.id = os.base_id;
+    objState->base.category = os.base_category;
+    objState->base.type = os.base_type;
+    objState->base.visMask = os.base_vis_mask;
+    std::strcpy(objState->base.name, os.base_name.c_str());
+    objState->base.geo = os.base_geo;
+    objState->base.pos = os.base_pos;
+    objState->ext.speed = os.ext_speed;
+    objState->ext.accel = os.ext_accel;
+    handler_.addPackage(0.0, 0, RDB_PKG_ID_END_OF_FRAME);
+  }
+
   /**
    * Add the trigger package, which specifies how much VTD should step.
    */

optional/vtd/src/vtd_data_conversion_test.cpp

@@ -0,0 +1,51 @@
+/*
+ * Copyright 2021 Robert Bosch GmbH
+
+ */
+/**
+ * \file vtd_data_conversion_test.cpp
+ * \see  omni_sensor_component.hpp
+ * \see  task_control.hpp
+ */
+
+#include <cmath>  // for M_PI
+
+#include <gtest/gtest.h>              // for TEST, ASSERT_TRUE
+#include <cloe/component/object.hpp>  // for Object
+
+#include "omni_sensor_component.hpp"
+#include "task_control.hpp"
+
+RDB_COORD_t get_test_rdb_coord() {
+  RDB_COORD_t c;
+  c.x = 1.0;
+  c.y = 2.0;
+  c.z = 3.0;
+  c.h = 0.1 * M_PI;
+  c.p = 0.2 * M_PI;
+  c.r = 0.3 * M_PI;
+  c.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
+  return c;
+}
+
+void assert_rdb_coord_eq(const RDB_COORD_t& c1, const RDB_COORD_t& c2) {
+  ASSERT_DOUBLE_EQ(c1.x, c2.x);
+  ASSERT_DOUBLE_EQ(c1.y, c2.y);
+  ASSERT_DOUBLE_EQ(c1.z, c2.z);
+  if (c2.flags & RDB_COORD_FLAG_ANGLES_VALID) {
+    ASSERT_DOUBLE_EQ(c1.h, c2.h);
+    ASSERT_DOUBLE_EQ(c1.p, c2.p);
+    ASSERT_DOUBLE_EQ(c1.r, c2.r);
+  }
+}
+
+TEST(vtd_data, rdb_coord) {
+  // Convert from VTD to Cloe data and back.
+  RDB_COORD_t coord = get_test_rdb_coord();
+  cloe::Object obj;
+  obj.pose = vtd::from_vtd_pose(coord);
+  RDB_COORD_t coord2 = vtd::rdb_coord_from_object(obj);
+  assert_rdb_coord_eq(coord, coord2);
+  coord2 = vtd::rdb_coord_pos_from_vector3d(obj.pose.translation());
+  assert_rdb_coord_eq(coord, coord2);
+}

optional/vtd/src/vtd_vehicle.hpp

@@ -230,28 +230,42 @@ class VtdVehicle : public cloe::Vehicle {
     for (const auto& comp : components) {
       auto name = comp.first;
       auto cfg = comp.second;
-      if (!sensors_.count(cfg.from)) {
-        throw cloe::ModelError("reference to unknown sensor '{}'", 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);
+      if (cfg.from.empty()) {
+        // Configure actuators.
+        if (cfg.type == "ego_state_model") {
+          auto ego_model = std::make_shared<VtdExternalEgoModel>(task_control_, id_, vtd_name_);
+          // Since no default component of this type was instantiated in the constructor, cfg.override does not apply.
+          this->add_component(ego_model, name);
+          ego_control_ = ego_model;
         } else {
-          this->add_component(std::shared_ptr<cloe::Component>(c), name);
+          throw cloe::ModelError("unknown actuator component type '{}'", cfg.type);
         }
-      };
-      if (cfg.type == "lane_sensor") {
-        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);
+        // Configure sensors.
+        std::shared_ptr<VtdSensorData> sensor;
+        if (cfg.from != "task_control") {
+          if (!sensors_.count(cfg.from)) {
+            throw cloe::ModelError("reference to unknown sensor '{}'", cfg.from);
+          }
+          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);
+          } else {
+            this->add_component(std::shared_ptr<cloe::Component>(c), name);
+          }
+        };
+        if (cfg.type == "lane_sensor") {
+          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 sensor component type '{}'", cfg.type);
+        }
       }
     }
   }