From c40d743b600111e0d3113959d9fa67e0bc7c4cc7 Mon Sep 17 00:00:00 2001 From: Franco Guidi Date: Mon, 29 Jan 2024 12:49:27 +0100 Subject: [PATCH] minimator: Add new schema for minimator - Create a basic EgoSensor and object sensor. - Add new schema to configure position and velocity of Ego and traffic object. - Adapt world object and minimator vehicle class accordingly. --- .../tests/config_minimator_smoketest.json | 32 +- .../tests/test_gndtruth_smoketest_output.json | 200 +++++++++++- .../test_gndtruth_smoketest_output.msgpack | Bin 5685 -> 6989 bytes plugins/minimator/CMakeLists.txt | 28 ++ plugins/minimator/conanfile.py | 3 + plugins/minimator/include/minimator.hpp | 158 ++++++++++ plugins/minimator/src/minimator.cpp | 293 ++++++++++++------ .../minimator/src/minimator_config_test.cpp | 144 +++++++++ .../tests/config_minimator_infinite.json | 37 ++- ...config_minimator_multi_agent_infinite.json | 80 ++++- .../tests/config_minimator_smoketest.json | 6 +- 11 files changed, 864 insertions(+), 117 deletions(-) create mode 100644 plugins/minimator/include/minimator.hpp create mode 100644 plugins/minimator/src/minimator_config_test.cpp diff --git a/plugins/gndtruth_extractor/tests/config_minimator_smoketest.json b/plugins/gndtruth_extractor/tests/config_minimator_smoketest.json index f6e51b69d..7edadbd8c 100644 --- a/plugins/gndtruth_extractor/tests/config_minimator_smoketest.json +++ b/plugins/gndtruth_extractor/tests/config_minimator_smoketest.json @@ -5,7 +5,35 @@ ], "simulators": [ { - "binding": "minimator" + "binding": "minimator", + "args": { + "vehicles": { + "ego1": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 20.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + } + } + } } ], "vehicles": [ @@ -13,7 +41,7 @@ "name": "default", "from": { "simulator": "minimator", - "index": 0 + "name": "ego1" } } ], diff --git a/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.json b/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.json index d2338c675..e54ff270e 100644 --- a/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.json +++ b/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.json @@ -25,8 +25,54 @@ "z": 0.0 } }, - "name": "nop_object_sensor", - "sensed_objects": [] + "name": "minimator_object_sensor", + "sensed_objects": [ + { + "acceleration": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular_velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "class": "unknown", + "cog_offset": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "dimensions": { + "x": 5.0, + "y": 2.0, + "z": 1.8 + }, + "exist_prob": 1.0, + "id": 0, + "pose": { + "rotation": { + "w": 1.0, + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "translation": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + }, + "type": "unknown", + "velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "velocity_norm": 0.0 + } + ] }, "cloe::gndtruth_lane_sensor": { "frustum": { @@ -188,8 +234,54 @@ "z": 0.0 } }, - "name": "nop_object_sensor", - "sensed_objects": [] + "name": "minimator_object_sensor", + "sensed_objects": [ + { + "acceleration": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular_velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "class": "unknown", + "cog_offset": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "dimensions": { + "x": 5.0, + "y": 2.0, + "z": 1.8 + }, + "exist_prob": 1.0, + "id": 0, + "pose": { + "rotation": { + "w": 1.0, + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "translation": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + }, + "type": "unknown", + "velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "velocity_norm": 0.0 + } + ] }, "cloe::gndtruth_lane_sensor": { "frustum": { @@ -351,8 +443,54 @@ "z": 0.0 } }, - "name": "nop_object_sensor", - "sensed_objects": [] + "name": "minimator_object_sensor", + "sensed_objects": [ + { + "acceleration": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular_velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "class": "unknown", + "cog_offset": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "dimensions": { + "x": 5.0, + "y": 2.0, + "z": 1.8 + }, + "exist_prob": 1.0, + "id": 0, + "pose": { + "rotation": { + "w": 1.0, + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "translation": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + }, + "type": "unknown", + "velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "velocity_norm": 0.0 + } + ] }, "cloe::gndtruth_lane_sensor": { "frustum": { @@ -514,8 +652,54 @@ "z": 0.0 } }, - "name": "nop_object_sensor", - "sensed_objects": [] + "name": "minimator_object_sensor", + "sensed_objects": [ + { + "acceleration": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular_velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "class": "unknown", + "cog_offset": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "dimensions": { + "x": 5.0, + "y": 2.0, + "z": 1.8 + }, + "exist_prob": 1.0, + "id": 0, + "pose": { + "rotation": { + "w": 1.0, + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "translation": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + }, + "type": "unknown", + "velocity": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "velocity_norm": 0.0 + } + ] }, "cloe::gndtruth_lane_sensor": { "frustum": { diff --git a/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.msgpack b/plugins/gndtruth_extractor/tests/test_gndtruth_smoketest_output.msgpack index 35ac923732abe689833d6890503fb9b959366ee7..98131e74af30d1c7f9fb0417cdb4909003a1a4ca 100644 GIT binary patch literal 6989 zcmeHLyN=W_6!q@%2^7#E_y8mx-8MTsBx)r3$QyemhQy9+XC6y~_CbRvK>Gm%%_9u# zK*3{0wI!N`kD#WZ2MMk{nN1d=(FhnSr%CROA2~NCK0evq8u}uN1*crA&G&vN=;|t= zb)1CSYY7<=Pf@Of+!@qmqI447&-{?ZULDKvwXX<0vk|8dA~jtKFmEIwmR^&F z!e_cuJ#Wtsu~LJC-{GRgp~?+!g1)->kgy1z1@J1$W6NGt~WyYF#;NXMssz7F*A*?!HMxZwZfl8R(=Jc_tb>J?+l2NstUU=B^MJQyi6);U-`iX?E0?6kFKptvKH9ATv zjit7^K52WDlkwFL2xV|x+{s8Mk7@@9m3d%Z-d;d4VZ2f5V5l+Rw5>fxe)MC(;QM}D zqMjKhf9ED^W;A>&vV&OCrs-@c(NiL1B)zymT!3RfTuU=?!7vrLitKS*eS%d$NqT8< zz@Vga0$ZL1%nb88uu`@f*yRNRgURQ6V=FU(!7vrTitKS<6DDSgMZk6%FWD;%i|66m z?#AaEqmy@?q&Z1LaO9+Nl4fdgY}aJ>L{8G2r2XL@1lHy`SAHjH#pdb&%9gK5GbL@g zBxzkIX+k=e_UroXT$+lmvR66eya*DlQSJhs|N8d;!ATPaD}X#eJyoiqE+r8$=d j+sNsT$hkCA3+K`ha+3Cs{uDb&Tco6&C`sDuiA(zl^md=C delta 78 ucmX?WwpC}sbC!*H`2`a{$}&#a?9Y_Vioz{onLLH}C%RC&2!@cAv@ie@U>`LA diff --git a/plugins/minimator/CMakeLists.txt b/plugins/minimator/CMakeLists.txt index 2a06b5db6..4986e1855 100644 --- a/plugins/minimator/CMakeLists.txt +++ b/plugins/minimator/CMakeLists.txt @@ -9,9 +9,37 @@ include(CloePluginSetup) cloe_add_plugin( TARGET ${PROJECT_NAME} OUTPUT_NAME simulator_minimator + INCLUDE_DIRECTORIES + $ SOURCES src/minimator.cpp LINK_LIBRARIES cloe::runtime cloe::models ) + +include(CTest) +if(BUILD_TESTING) + find_package(GTest REQUIRED) + include(GoogleTest) + + set(test-minimator test-${PROJECT_NAME}) + add_executable(${test-minimator} + src/minimator_config_test.cpp + ) + target_include_directories(${test-minimator} + PUBLIC + $ + ) + set_target_properties(${test-minimator} PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED ON + ) + target_link_libraries(${test-minimator} + GTest::gtest + GTest::gtest_main + cloe::runtime + cloe::models + ) + gtest_add_tests(TARGET ${test-minimator}) +endif() diff --git a/plugins/minimator/conanfile.py b/plugins/minimator/conanfile.py index 0b3f57f56..b0a59af0f 100644 --- a/plugins/minimator/conanfile.py +++ b/plugins/minimator/conanfile.py @@ -25,6 +25,7 @@ class CloeSimulatorMinimator(ConanFile): generators = "CMakeDeps", "VirtualRunEnv" no_copy_source = True exports_sources = [ + "include/*", "src/*", "CMakeLists.txt", ] @@ -60,6 +61,8 @@ def build(self): cm.configure() if self.should_build: cm.build() + if self.should_test: + cm.test() def package(self): cm = cmake.CMake(self) diff --git a/plugins/minimator/include/minimator.hpp b/plugins/minimator/include/minimator.hpp new file mode 100644 index 000000000..cdf58106f --- /dev/null +++ b/plugins/minimator/include/minimator.hpp @@ -0,0 +1,158 @@ +/* + * Copyright 2023 Robert Bosch GmbH + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include // for Confable, Schema, ... + +namespace minimator { + +struct ObjectPosition : public cloe::Confable { + double x{0.0}; + double y{0.0}; + double z{0.0}; + + public: + CONFABLE_SCHEMA(ObjectPosition) { + return cloe::Schema{ + {"x", cloe::Schema(&x, "Object position x axis")}, + {"y", cloe::Schema(&y, "Object position y axis")}, + {"z", cloe::Schema(&z, "Object position z axis")}, + }; + } +}; + +struct ObjectConfig : public cloe::Confable { + double velocity{0.0}; + ObjectPosition position; + + public: + CONFABLE_SCHEMA(ObjectConfig) { + return cloe::Schema{ + {"velocity", cloe::Schema(&velocity, "Object longitudinal velocity")}, + {"position", cloe::Schema(&position, "Object position coordinates (x,y,z)")}, + }; + } +}; + +struct ObjectSensorConfig : public cloe::Confable { + std::vector objects; + + public: + CONFABLE_SCHEMA(ObjectSensorConfig) { + // clang-format off + return cloe::Schema{ + {"objects", cloe::Schema(&objects, "Array of object configuration relative to ego vehicle")}, + }; + // clang-format on + } +}; + +struct EgoSensorConfig : public cloe::Confable { + ObjectConfig ego_object; + + public: + CONFABLE_SCHEMA(EgoSensorConfig) { + return cloe::Schema{ + {"ego_object", cloe::Schema(&ego_object, "Ego object configuration in world coordinates")}, + }; + } +}; + +struct SensorMockupConfig : public cloe::Confable { + EgoSensorConfig ego_sensor_mockup; + ObjectSensorConfig object_sensor_mockup; + + public: + CONFABLE_SCHEMA(SensorMockupConfig) { + // clang-format off + return cloe::Schema{ + {"ego_sensor_mockup", cloe::Schema(&ego_sensor_mockup, "Ego sensor mockup configuration")}, + {"object_sensor_mockup", cloe::Schema(&object_sensor_mockup, "Object sensor mockup configuration")}, + }; + // clang-format on + } +}; + +/** + * MinimatorConfiguration is what we use to configure `Minimator` from some + * JSON input. + * + * The Cloe runtime takes care of reading the configuration from the stack + * file and passing it to the `MinimatorFactory`, which can then pass it + * to `Minimator` during construction. + * + * So, the input will be deserialized from `/simulators/N/args`, where `N` is + * some entry in the `simulators` object: + * + * { + * "version": "4", + * "simulators": [ + * { + * "binding": "minimator" + * "args": { + * "vehicles": [ + * "ego1", + * "ego2" + * ] + * } + * } + * ] + * } + * + * Since our minimalistic simulator doesn't do much yet, our configuration + * is quite simple: a number of names which will each become a vehicle. + */ + +struct MinimatorConfiguration : public cloe::Confable { + std::map vehicles; + // The `CONFABLE_SCHEMA` is simple enough and is the recommended way to + // augment a class that inherits from `Confable` to expose `from_conf`, + // `from_json`, and `to_json` methods. The `Schema` type is a sort of + // polymorphic type that automatically derives a JSON schema from a set of + // pointers. This schema is used to provide serialization and deserialization. + // + // \see fable/confable.hpp + // \see fable/schema.hpp + // + public: + CONFABLE_SCHEMA(MinimatorConfiguration) { + // For us, each `Schema` describing a `Confable` will start with an + // initializer list of pairs: this describes a JSON object. Each property in + // this object may be another object or another primitive JSON type. + // In this configuration, we want to deserialize into a vector of strings. + // + // `Schema` contains some magic to make it "easy" for you to use. + // The following eventually boils down to: + // + // fable::schema::Struct{ + // { + // "vehicles", + // fable::schema::map( + // &vehicles, + // "list of vehicle names to make available" + // ) + // } + // } + // + // You can hopefully see why `Schema` contains the magic it contains. + return cloe::Schema{ + {"vehicles", cloe::Schema(&vehicles, "list of vehicle names to make available")}, + }; + } +}; + +} diff --git a/plugins/minimator/src/minimator.cpp b/plugins/minimator/src/minimator.cpp index 7c426abd7..7dbc5bee4 100644 --- a/plugins/minimator/src/minimator.cpp +++ b/plugins/minimator/src/minimator.cpp @@ -53,86 +53,24 @@ #include // for string #include // for vector<> -#include // for NopEgoSensor -#include // for LaneBoundarySensor -#include // for LatLongActuator -#include // for NopObjectSensor -#include // for ToJson -#include // for CloeComponent -#include // for EXPORT_CLOE_PLUGIN -#include // for Registrar -#include // for Simulator -#include // for Sync -#include // for Vehicle +#include // for NopEgoSensor +#include // for LaneBoundarySensor +#include // for LatLongActuator +#include // for NopObjectSensor +#include // for ObjectSensorFilter +#include // for ToJson +#include // for CloeComponent +#include // for EXPORT_CLOE_PLUGIN +#include // for Registrar +#include // for Simulator +#include // for Sync +#include // for pose_from_rotation_translation +#include // for Vehicle +#include namespace minimator { -/** - * MinimatorConfiguration is what we use to configure `Minimator` from some - * JSON input. - * - * The Cloe runtime takes care of reading the configuration from the stack - * file and passing it to the `MinimatorFactory`, which can then pass it - * to `Minimator` during construction. - * - * So, the input will be deserialized from `/simulators/N/args`, where `N` is - * some entry in the `simulators` object: - * - * { - * "version": "4", - * "simulators": [ - * { - * "binding": "minimator" - * "args": { - * "vehicles": [ - * "ego1", - * "ego2" - * ] - * } - * } - * ] - * } - * - * Since our minimalistic simulator doesn't do much yet, our configuration - * is quite simple: a number of names which will each become a vehicle. - */ -struct MinimatorConfiguration : public cloe::Confable { - std::vector vehicles{"default"}; - - // The `CONFABLE_SCHEMA` is simple enough and is the recommended way to - // augment a class that inherits from `Confable` to expose `from_conf`, - // `from_json`, and `to_json` methods. The `Schema` type is a sort of - // polymorphic type that automatically derives a JSON schema from a set of - // pointers. This schema is used to provide serialization and deserialization. - // - // \see fable/confable.hpp - // \see fable/schema.hpp - // - CONFABLE_SCHEMA(MinimatorConfiguration) { - // For us, each `Schema` describing a `Confable` will start with an - // initializer list of pairs: this describes a JSON object. Each property in - // this object may be another object or another primitive JSON type. - // In this configuration, we want to deserialize into a vector of strings. - // - // `Schema` contains some magic to make it "easy" for you to use. - // The following eventually boils down to: - // - // fable::schema::Struct{ - // { - // "vehicles", - // fable::schema::Array( - // &vehicles, - // "list of vehicle names to make available" - // ) - // } - // } - // - // You can hopefully see why `Schema` contains the magic it contains. - return cloe::Schema{ - {"vehicles", cloe::Schema(&vehicles, "list of vehicle names to make available")}, - }; - } -}; +using Vehicles = std::vector>; /** * MinimatorLaneSensor is a very static lane boundary sensor. @@ -185,7 +123,150 @@ class MinimatorLaneSensor : public cloe::LaneBoundarySensor { private: cloe::LaneBoundaries lane_boundaries_; cloe::Frustum frustum_; - Eigen::Isometry3d mount_pose_ = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d mount_pose_; +}; + +/** + * The SimulatorSensorMockup class provides dummy values for the World sensor and Ego sensor. + * The Ego Sensor values are provided with respect to the Cloe frame (center of the + * rear axle). + * The world sensor values are provided relative to the sensor mounting position. +*/ +class SimulatorSensorMockup { + public: + SimulatorSensorMockup(const uint16_t& id, const SensorMockupConfig& sensor_mockup_config) + : vehicle_id(id), sensor_mockup_config_(sensor_mockup_config) {} + + void process_ego_vehicles() { + std::array ori{0.0, 0.0, 0.0}; + std::array pos{sensor_mockup_config_.ego_sensor_mockup.ego_object.position.x, + sensor_mockup_config_.ego_sensor_mockup.ego_object.position.y, + sensor_mockup_config_.ego_sensor_mockup.ego_object.position.z}; + if (ego_objects_.empty()) { + auto o = std::make_shared(); + o->id = this->vehicle_id; + o->dimensions = Eigen::Vector3d(5.0, 2.00, 1.8); + Eigen::Quaterniond quaternion = cloe::utility::quaternion_from_rpy(ori[0], ori[1], ori[2]); + Eigen::Vector3d translation = Eigen::Vector3d(pos[0], pos[1], pos[2]); + o->pose = cloe::utility::pose_from_rotation_translation(quaternion, translation); + o->velocity = + Eigen::Vector3d(sensor_mockup_config_.ego_sensor_mockup.ego_object.velocity, 0.0, 0.0); + ego_objects_.emplace_back(std::move(o)); + } + } + + void process_sensed_objects() { + if (world_objects_.empty()) { + for (const auto& object : sensor_mockup_config_.object_sensor_mockup.objects) { + std::array ori{0.0, 0.0, 0.0}; + std::array pos{object.position.x, object.position.y, object.position.z}; + auto o = std::make_shared(); + o->id = this->vehicle_id; + o->dimensions = Eigen::Vector3d(5.0, 2.00, 1.8); + Eigen::Quaterniond quaternion = cloe::utility::quaternion_from_rpy(ori[0], ori[1], ori[2]); + Eigen::Vector3d translation = Eigen::Vector3d(pos[0], pos[1], pos[2]); + o->pose = cloe::utility::pose_from_rotation_translation(quaternion, translation); + if (object.velocity != 0.0f) { + // This plugin is only meant to be a minimalistic implementation to show what is Cloe capable of doing. + // This Minimator plugin does not implement yet a proper model to handle different cases. + // If desired to implement velocity and acceleration in the traffic object, the logic to calculate the + // position of the object should be extended. + throw cloe::ModelError( + "Unsupported value: {}.\n We don't currently support longitudinal velocity with " + "respect to ego vehicle " + "different than zero for traffic " + "objects.", + object.velocity); + } + o->velocity = Eigen::Vector3d(object.velocity, 0.0, 0.0); + world_objects_.emplace_back(std::move(o)); + } + } + } + + void process() { + process_ego_vehicles(); + process_sensed_objects(); + } + + const cloe::Objects& get_world_objects() const { return world_objects_; } + const cloe::Objects& get_ego_objects() const { return ego_objects_; } + const cloe::Object& get_object(const cloe::Objects& objects) const { + auto object_it = std::find_if(objects.begin(), objects.end(), + [this](std::shared_ptr o) -> bool { + return this->vehicle_id == static_cast(o->id); + }); + + if (object_it == objects.end()) { + throw std::invalid_argument(fmt::format("Unknown vehicle id {}", this->vehicle_id)); + } + + return **object_it; + } + const cloe::Object& get_world_object() const { return this->get_object(get_world_objects()); } + const cloe::Object& get_ego_object() const { return this->get_object(get_ego_objects()); } + + /** + * Serialize World into JSON. + * + * This is required for the `ToJson` handler that is used in the `enroll` + * method. + */ + friend void to_json(cloe::Json& j, const SimulatorSensorMockup& b) { + j = cloe::Json{ + {"vehicles", b.ego_objects_}, + {"objects", b.world_objects_}, + }; + } + + private: + uint16_t vehicle_id; + cloe::Objects world_objects_; + cloe::Objects ego_objects_; + SensorMockupConfig sensor_mockup_config_; +}; + +class MinimatorObjectSensor : public cloe::ObjectSensor { + public: + MinimatorObjectSensor(cloe::Objects world_objects) + : cloe::ObjectSensor("minimator_object_sensor"), world_objects_(std::move(world_objects)) { + mount_pose_.setIdentity(); + } + + const cloe::Objects& sensed_objects() const override { return world_objects_; } + + const cloe::Frustum& frustum() const override { return frustum_; } + + const Eigen::Isometry3d& mount_pose() const override { return mount_pose_; } + + private: + cloe::Frustum frustum_; + Eigen::Isometry3d mount_pose_; + cloe::Objects world_objects_; +}; + +/** + * MinimatorEgoSensor is a minimalistic EgoSensor. + */ +class MinimatorEgoSensor : public cloe::EgoSensor { + public: + using cloe::EgoSensor::EgoSensor; + MinimatorEgoSensor(uint16_t id, cloe::Object ego_object) + : EgoSensor("minimator_ego_sensor"), id_(id), ego_object_(std::move(ego_object)) {} + virtual ~MinimatorEgoSensor() noexcept = default; + const cloe::Object& sensed_state() const override { return ego_object_; } + double wheel_steering_angle() const override { return angle_; } + virtual double steering_wheel_speed() const override { return steering_wheel_speed_; } + void reset() override { + angle_ = 0.0; + steering_wheel_speed_ = 0.0; + } + + protected: + uint16_t id_; + cloe::Object ego_object_; + double angle_{0.0}; + double steering_wheel_speed_{0.0}; }; /** @@ -214,6 +295,8 @@ class MinimatorVehicle : public cloe::Vehicle { * * \arg id unique ID within simulator's set of vehicles * \arg name unique name within simulator's set of vehicles + * \arg simulator_sensor_mockup world object containing list of ego vehicles and + * traffic objects, simulating data coming from a simulator. * * ## Components * @@ -239,7 +322,9 @@ class MinimatorVehicle : public cloe::Vehicle { * \see cloe/component/object_sensor.hpp * \see cloe/component/latlong_actuator.hpp */ - MinimatorVehicle(uint64_t id, const std::string& name) : Vehicle(id, name) { + MinimatorVehicle(uint16_t id, const std::string& name, + SimulatorSensorMockup& simulator_sensor_mockup) + : Vehicle(id, name), simulator_sensor_mockup_(simulator_sensor_mockup) { // Create a new `EgoSensor` and store it in the vehicle, making it available // by the standard names as defined by the enum values `DEFAULT_EGO_SENSOR` // and `GROUNDTRUTH_EGO_SENSOR`. @@ -248,12 +333,13 @@ class MinimatorVehicle : public cloe::Vehicle { // If you want more control, use `set_component` or `add_component`. // // \see Vehicle::new_component - this->new_component(new cloe::NopEgoSensor(), + + this->new_component(new MinimatorEgoSensor(id, simulator_sensor_mockup_.get_ego_object()), cloe::CloeComponent::GROUNDTRUTH_EGO_SENSOR, cloe::CloeComponent::DEFAULT_EGO_SENSOR); // Similarly here. - this->new_component(new cloe::NopObjectSensor(), + this->new_component(new MinimatorObjectSensor(simulator_sensor_mockup_.get_world_objects()), cloe::CloeComponent::GROUNDTRUTH_WORLD_SENSOR, cloe::CloeComponent::DEFAULT_WORLD_SENSOR); @@ -281,6 +367,9 @@ class MinimatorVehicle : public cloe::Vehicle { * \see Vehicle::process */ cloe::Duration process(const cloe::Sync& sync) final { return Vehicle::process(sync); } + + protected: + SimulatorSensorMockup& simulator_sensor_mockup_; }; /** @@ -349,8 +438,14 @@ class MinimatorSimulator : public cloe::Simulator { // For each of the vehicle names in the configuration, create a new // vehicle. We are responsible for ensuring that the vehicles are alive // for the duration of a simulation. We use `std::shared_ptr` for this. - for (size_t i = 0; i < config_.vehicles.size(); i++) { - vehicles_.emplace_back(std::make_shared(i, config_.vehicles[i])); + uint16_t id{0}; + for (const auto& [vehicle_name, sensor_mockup] : config_.vehicles) { + SimulatorSensorMockup simulator_sensor_mockup_(id, sensor_mockup); + simulator_sensor_mockup_.process(); + vehicles.emplace_back( + std::make_shared(id, vehicle_name, simulator_sensor_mockup_)); + vehicles_data.emplace_back(simulator_sensor_mockup_); + ++id; } } @@ -367,7 +462,7 @@ class MinimatorSimulator : public cloe::Simulator { assert(!is_operational()); // Empty the list of vehicles. - vehicles_.clear(); + vehicles.clear(); // Also call superclass method. Simulator::disconnect(); @@ -447,8 +542,8 @@ class MinimatorSimulator : public cloe::Simulator { // deserializing into JSON. This is automatically provided by the // `Confable` type, but for `MinimatorSimulator` we have to define it // ourself. - r.register_api_handler( - "/state", cloe::HandlerType::BUFFERED, cloe::handler::ToJson(this)); + r.register_api_handler("/state", cloe::HandlerType::BUFFERED, + cloe::handler::ToJson(this)); r.register_api_handler("/configuration", cloe::HandlerType::BUFFERED, cloe::handler::ToJson(&config_)); @@ -463,7 +558,7 @@ class MinimatorSimulator : public cloe::Simulator { */ size_t num_vehicles() const final { assert(is_connected()); - return vehicles_.size(); + return vehicles.size(); } /** @@ -473,7 +568,7 @@ class MinimatorSimulator : public cloe::Simulator { */ std::shared_ptr get_vehicle(size_t i) const final { assert(i < num_vehicles()); - return vehicles_[i]; + return vehicles[i]; } /** @@ -482,7 +577,7 @@ class MinimatorSimulator : public cloe::Simulator { * \see Simulator::get_vehicle */ std::shared_ptr get_vehicle(const std::string& key) const final { - for (const auto& v : vehicles_) { + for (const auto& v : vehicles) { if (v->name() == key) { return v; } @@ -509,8 +604,14 @@ class MinimatorSimulator : public cloe::Simulator { assert(is_connected()); assert(is_operational()); - // Our simulator here doesn't really do anything at all, so we can keep - // running forever. + // Fetch new data and save it in vehicle list. + for (auto& d : vehicles_data) { + d.process(); + // Note: Our simulator here doesn't really do anything at all, so we can keep + // running forever. + // Here there would be the logic to set the vehicle data that is read in this process() call. + } + return sync.time(); } @@ -522,15 +623,17 @@ class MinimatorSimulator : public cloe::Simulator { */ friend void to_json(cloe::Json& j, const MinimatorSimulator& b) { j = cloe::Json{ - {"is_connected", b.connected_}, {"is_operational", b.operational_}, - {"running", nullptr}, {"num_vehicles", b.num_vehicles()}, - {"vehicles", b.vehicles_}, + {"is_connected", b.connected_}, + {"is_operational", b.operational_}, + {"running", nullptr}, + {"num_vehicles", b.num_vehicles()}, }; } private: MinimatorConfiguration config_; - std::vector> vehicles_; + Vehicles vehicles; + std::vector vehicles_data; }; // The plugin manifest we will define at the end of this file requires diff --git a/plugins/minimator/src/minimator_config_test.cpp b/plugins/minimator/src/minimator_config_test.cpp new file mode 100644 index 000000000..4250c81bd --- /dev/null +++ b/plugins/minimator/src/minimator_config_test.cpp @@ -0,0 +1,144 @@ +/* + * Copyright 2023 Robert Bosch GmbH + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include // for assert_validate + +#include // for assert_validate +#include + +namespace minimator { + +TEST(minimator, deserialize_object_position) { + ObjectPosition pos; + + fable::assert_validate(pos, R"({ + "x": 0.0, + "y": 0.0, + "z": 0.0 + })"); +} + +TEST(minimator, deserialize_object_config) { + ObjectConfig obj_conf; + + fable::assert_validate(obj_conf, R"({ + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "velocity" : 0.0 + })"); +} + +TEST(minimator, deserialize_object_sensor_config) { + ObjectSensorConfig obj_sensor_conf; + + fable::assert_validate(obj_sensor_conf, R"({ + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + ] + })"); +} + +TEST(minimator, deserialize_ego_sensor_config) { + EgoSensorConfig ego_sensor_conf; + + fable::assert_validate(ego_sensor_conf, R"({ + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + })"); +} + +TEST(minimator, deserialize_sensor_mockup_config) { + SensorMockupConfig sensor_mockup_conf; + + fable::assert_validate(sensor_mockup_conf, R"({ + "ego_sensor_mockup": { + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + })"); +} + +TEST(minimator, deserialize_minimator_config) { + MinimatorConfiguration minimator_config; + + fable::assert_validate(minimator_config, R"({ + "vehicles": { + "ego1": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + } + } + })"); +} + +} // namespace minimator diff --git a/plugins/minimator/tests/config_minimator_infinite.json b/plugins/minimator/tests/config_minimator_infinite.json index 89e1c8aca..c345c1f64 100644 --- a/plugins/minimator/tests/config_minimator_infinite.json +++ b/plugins/minimator/tests/config_minimator_infinite.json @@ -8,15 +8,44 @@ { "binding": "virtue", "args": { - "lane_sensor_components": ["cloe::default_lane_sensor"] + "lane_sensor_components": [ + "cloe::default_lane_sensor" + ] } } ] }, "simulators": [ { - "binding": "minimator" - // name is automatically "minimator" + "binding": "minimator", + "args": { + "vehicles": { + "ego1": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 20.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + } + } + } } ], "vehicles": [ @@ -24,7 +53,7 @@ "name": "default", "from": { "simulator": "minimator", - "index": 0 + "name": "ego1" }, "components": { "cloe::speedometer": { diff --git a/plugins/minimator/tests/config_minimator_multi_agent_infinite.json b/plugins/minimator/tests/config_minimator_multi_agent_infinite.json index e8413d9f7..e1698e7a2 100644 --- a/plugins/minimator/tests/config_minimator_multi_agent_infinite.json +++ b/plugins/minimator/tests/config_minimator_multi_agent_infinite.json @@ -4,11 +4,81 @@ { "binding": "minimator", "args": { - "vehicles": [ - "first", - "second", - "third" - ]} + "vehicles": { + "first": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + }, + "second": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + }, + "third": { + "ego_sensor_mockup": { + "ego_object": { + "velocity": 0.0, + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + } + }, + "object_sensor_mockup": { + "objects": [ + { + "velocity": 0.0, + "position": { + "x": 10.0, + "y": 0.0, + "z": 0.0 + } + } + ] + } + } + } + } } ], "vehicles": [ diff --git a/plugins/minimator/tests/config_minimator_smoketest.json b/plugins/minimator/tests/config_minimator_smoketest.json index 767d78985..c59072066 100644 --- a/plugins/minimator/tests/config_minimator_smoketest.json +++ b/plugins/minimator/tests/config_minimator_smoketest.json @@ -4,14 +4,14 @@ "config_minimator_infinite.json" ], "server": { - "listen": false, + "listen": true, "listen_port": 23456 }, "triggers": [ {"event": "virtue/failure", "action": "fail"}, { - "label": "Vehicle default should never move with the minimator binding.", - "event": "default_speed/kmph=>0.0", "action": "fail" + "label": "Vehicle default should never move at a velocity greater that 72 kmph with the minimator binding.", + "event": "default_speed/kmph=>80.0", "action": "fail" }, {"event": "start", "action": "log=info: Running minimator/basic smoketest."}, {"event": "start", "action": "realtime_factor=-1"},