diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index a286c303ac..a86c0635d8 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -10,6 +10,13 @@ // 4. source ~/.scrimmage/setup.bash // 5. scrimmage missions/straight-no-gui.xml // +// To persist keys across reboots: +// macOS: ssh-add --apple-use-keychain ~/.ssh/id_rsa +// Linux: Add `AddKeysToAgent yes` to ~/.ssh/config +// Windows (PowerShell as Admin): +// Get-Service ssh-agent | Set-Service -StartupType Automatic -PassThru | Start-Service +// ssh-add $HOME\.ssh\id_rsa +// { "name": "SCRIMMAGE Dev Container", "build": { @@ -22,6 +29,11 @@ "containerEnv": { "DISPLAY": "${localEnv:DISPLAY}" }, + "features": { + "ghcr.io/devcontainers/features/git:1": {}, + "ghcr.io/devcontainers/features/sshd:1": {} + }, + "initializeCommand": "ssh-add -l || true", "customizations": { "vscode": { "extensions": [ diff --git a/docs/source/tutorials/gen-entities.rst b/docs/source/tutorials/gen-entities.rst index 19e7177103..8d67fa9c39 100644 --- a/docs/source/tutorials/gen-entities.rst +++ b/docs/source/tutorials/gen-entities.rst @@ -99,52 +99,99 @@ by adding an entity block key-value pair to the ``GenerateEntity`` message: // Publish the GenerateEntity message pub_gen_ents_->publish(msg); -Modify Plugin Parameters ------------------------- +Plugin Parameter Overrides +-------------------------- -Before publishing the message, you can modify the XML attributes of specific plugins -by adding an entity plugin key-value-attr block to the ``GenerateEntity`` message: +When one entity spawns others at runtime, each spawned entity can receive +different plugin parameters. Without this, spawning many entities with slight +parameter variations would require duplicating entity blocks in the mission XML +(resulting in huge files) or writing complex nested XML by hand. -.. code-block:: c++ +A spawner entity (e.g., a carrier, hive, or RL controller) publishes +``GenerateEntity`` messages referencing a template. Each message can include +parameter overrides, so one template supports thousands of parameter variations. - // Modify the entity's plugin speed - auto autonomy_speed = msg->data.add_plugin_param(); - autonomy_speed->set_plugin_type("autonomy0"); - autonomy_speed->set_tag_name("speed"); - autonomy_speed->set_tag_value("100") - -Here the ``plugin_type`` represents the type of plugin, like ``motion_model``, ``autonomy`` (Note the -autonomy plugin is referenced as ``autonomy#`` by the GUI, where ``autonomy0`` is the first instance -and increments if there are multiple autonomy plugins in one entity), or ``controller``. The ``tag_name`` -represents the plugin specific tag that should be updated, like ``speed`` for the Straight ``autonomy`` -plugin. The ``tag_value`` represents the value of the corresponding plugin specific tag. - -The above change in the plugin parameter for the ``GenerateEntity`` message does not affect -the stored parsed Mission XML plugin attribute value used by ``SimControl.cpp``. For example, -if the above block was only executed for a given conditional, entities without a block changing -their speed would default to the Mission XML defined speed. - -Another way to generate entities with different plugin parameters involves either using multiple entity -blocks with different entity ``tags`` or creating new plugin XML files that have differently -configured default parameters. For example, you could copy and rename the ``Straight.xml`` file to -``MyStraight.xml`` and then modify the plugin parameters in ``MyStraight.xml``. -In your entity, you can load the ``MyStraight`` autonomy plugin by referencing it directly as long -as it is in your ``SCRIMMAGE_PLUGIN_PATH``: +For example, a carrier entity might spawn drones with varying speeds: .. code-block:: xml - - 0 + + + 1 + MySpawnerPlugin ... - MyStraight + + + + + 0 + Straight ... -In your entity generation plugin, you can modify the ``autonomy`` tag before -publishing the ``GenerateEntity`` message: +The spawner plugin publishes messages that override parameters per-spawn: .. code-block:: c++ - auto kv_autonomy = msg->data.add_entity_param(); - kv_autonomy->set_key("autonomy"); - kv_autonomy->set_value("MyStraight2"); // Load parameters from MyStraight2.xml + auto msg = std::make_shared>(); + msg->data.set_entity_tag("drone"); // Reference the template + + // Override speed for the Straight autonomy plugin + auto auto_override = msg->data.add_plugin_override(); + auto_override->set_plugin_type("autonomy"); + auto_override->set_plugin_name("Straight"); + + auto speed = auto_override->add_params(); + speed->set_key("speed"); + speed->set_value("30"); // Each spawn can use a different value + + pub_gen_ents_->publish(msg); + +Alternatively, use ``plugin_index`` to target by position (0 = first, 1 = second, etc.): + +.. code-block:: c++ + + auto_override->set_plugin_type("autonomy"); + auto_override->set_plugin_index(0); + +This works from any plugin type — autonomy, sensor, controller, etc. External +projects just publish the message; all validation happens in base SCRIMMAGE. + +Validation +~~~~~~~~~~ + +By default, override keys must already exist in the plugin XML configuration or +as inline mission attributes for that plugin instance. Unknown keys are rejected +to catch typos early. + +For loose runtime overrides (e.g., prototyping), add to the mission: + +.. code-block:: xml + + false + +.. warning:: + + Attempting to override plugin names (``autonomy``, ``controller``, + ``motion_model``, ``sensor``) via ``entity_param`` will be ignored. + Plugins are determined by the entity template, not at runtime. + +.. note:: + + Use ``plugin_name`` to target plugins by name — clearer and refactor-safe. + Use ``plugin_index`` when multiple plugins share the same name, or when + you specifically need positional addressing. Index uses declaration order: + the first ``sensor`` is index ``0``, the second is index ``1``, etc. + +.. note:: + + SCRIMMAGE runs **all** plugins of each type every timestep — all autonomies, + all controllers, and all sensors execute in declaration order. Only + ``motion_model`` is singular (one per entity, always index ``0``). + +.. note:: + + For a higher-volume example, see + ``missions/test/test_generate_entity_runtime_override_stress.xml``. + That mission spawns 120 entities from one template, each with distinct + runtime parameter overrides. diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index 48b19fdf3a..cc53cbb0b5 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -46,6 +46,7 @@ #include "scrimmage/common/ID.h" #include "scrimmage/entity/Contact.h" +#include "scrimmage/entity/RuntimePluginOverrides.h" #include "scrimmage/fwd_decl.h" #include "scrimmage/proto/Visual.pb.h" #include "scrimmage/pubsub/Message.h" @@ -61,14 +62,12 @@ namespace scrimmage { using Service = std::function; -typedef std::map> AttributeMap; - struct EntityInitParams { EntityInitParams() {}; - AttributeMap overrides; GPUControllerPtr gpu_controller; GPUMotionModelPtr gpu_motion_model; std::map info; + RuntimePluginOverrides runtime_plugin_overrides; int id; int ent_desc_id; std::set plugin_tags; @@ -87,8 +86,7 @@ class Entity : public std::enable_shared_from_this { bool parse_visual( std::map& info, - MissionParsePtr mp, - std::map& overrides); + MissionParsePtr mp); void close(double t); void collision(); diff --git a/include/scrimmage/entity/EntityPluginHelper.h b/include/scrimmage/entity/EntityPluginHelper.h index dc5fbcb9c2..64cfea7140 100644 --- a/include/scrimmage/entity/EntityPluginHelper.h +++ b/include/scrimmage/entity/EntityPluginHelper.h @@ -56,7 +56,7 @@ template boost::optional> make_autonomy( const std::string& autonomy_name, PluginManagerPtr plugin_manager, - std::map& overrides, + const std::map& overrides, EntityPtr parent, StatePtr state, std::shared_ptr>& id_to_team_map, diff --git a/include/scrimmage/entity/RuntimePluginOverrides.h b/include/scrimmage/entity/RuntimePluginOverrides.h new file mode 100644 index 0000000000..4b8ab2b7b2 --- /dev/null +++ b/include/scrimmage/entity/RuntimePluginOverrides.h @@ -0,0 +1,114 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Ethan M Boos + * @date 13 April 2026 + * @version 0.1.0 + * @brief Runtime plugin parameter override types for dynamic entity spawning. + * @section DESCRIPTION + * Type aliases and utility functions for passing per-spawn plugin parameter + * overrides through the GenerateEntity mechanism. Used by SimControl and + * Entity::init(). + * + */ + +#ifndef INCLUDE_SCRIMMAGE_ENTITY_RUNTIMEPLUGINOVERRIDES_H_ +#define INCLUDE_SCRIMMAGE_ENTITY_RUNTIMEPLUGINOVERRIDES_H_ + +#include +#include +#include + +#include "scrimmage/fwd_decl.h" + +// Forward declarations for protobuf types +namespace scrimmage_msgs { +class GenerateEntity; +} + +namespace scrimmage { + +struct EntityPluginInfo; + +// Runtime plugin parameter overrides for dynamically spawned entities. +// Used by GenerateEntity to pass per-spawn parameter changes to Entity::init(). +// +// PluginParamMap: key-value pairs for a single plugin instance +// Example: {"speed": "25", "show_camera_images": "true"} +// +// PluginInstanceParamOverrides: map from plugin index to its param overrides +// Example: {0: {"speed": "25"}, 1: {"aggressive": "true"}} +// (autonomy[0] gets speed=25, autonomy[1] gets aggressive=true) +// +// RuntimePluginOverrides: map from plugin type to instance overrides +// Example: { +// "autonomy": {0: {"speed": "25"}}, +// "sensor": {0: {"max_range": "100"}, 1: {"fov": "90"}}, +// "motion_model": {0: {"max_speed": "50"}} +// } +using PluginParamMap = std::map; +using PluginInstanceParamOverrides = std::map; +using RuntimePluginOverrides = std::map; + +// Check if a key is trying to override which plugin is used (e.g., "autonomy=Foo"). +// Overriding plugin parameters is allowed (autonomy0.speed=25), but swapping out +// the plugin itself is not — the entity_tag defines plugin composition. +bool is_plugin_name_override_key(const std::string& key); + +// Parse runtime plugin overrides from a GenerateEntity protobuf message. +// Resolves plugin_name to plugin_index when specified. If plugin_name matches +// multiple plugins of the same type, an error is logged. Populates +// runtime_plugin_overrides keyed by (plugin_type, plugin_index). +// Returns true if parsing succeeded, false if errors occurred. +bool parse_runtime_plugin_overrides( + const scrimmage_msgs::GenerateEntity& data, + MissionParsePtr mp, + int ent_desc_id, + RuntimePluginOverrides& runtime_plugin_overrides); + +// Build the set of parameter names valid for runtime overrides. +// Combines params from the plugin's XML config and mission inline attributes. +// Populates allowed_param_names. Returns true if successful. +bool get_allowed_runtime_plugin_param_names( + const EntityPluginInfo& plugin_info, + FileSearch& file_search, + std::set& allowed_param_names); + +// Validate runtime plugin overrides against the mission and plugin configs. +// Checks that plugin indices exist and parameter names are declared. +// Returns true if valid, false if validation errors occurred. +bool validate_runtime_plugin_overrides( + MissionParsePtr mp, + FileSearchPtr file_search, + int ent_desc_id, + const RuntimePluginOverrides& runtime_plugin_overrides); + +// Merge runtime overrides into a plugin's base params. +// Starts with plugin_info.params and overlays any matching runtime overrides. +// Returns the merged parameter map ready for plugin initialization. +std::map resolve_plugin_params( + const EntityPluginInfo& plugin_info, + const RuntimePluginOverrides& runtime_plugin_overrides); + +} // namespace scrimmage + +#endif // INCLUDE_SCRIMMAGE_ENTITY_RUNTIMEPLUGINOVERRIDES_H_ diff --git a/include/scrimmage/fwd_decl.h b/include/scrimmage/fwd_decl.h index c1bced7af9..d786962920 100644 --- a/include/scrimmage/fwd_decl.h +++ b/include/scrimmage/fwd_decl.h @@ -164,8 +164,6 @@ using EntityInteractionPtr = std::shared_ptr; class Metrics; using MetricsPtr = std::shared_ptr; -using AttributeMap = std::map>; - class CameraInterface; } // namespace scrimmage diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 5f8c322c08..0dcb7a7803 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -45,8 +45,6 @@ #include "scrimmage/proto/Color.pb.h" #include "scrimmage/proto/Visual.pb.h" -namespace sp = scrimmage_proto; - namespace scrimmage { // Key 1: Entity Description XML ID @@ -77,6 +75,20 @@ struct GenerateInfo { double time_variance; }; +/// Structured metadata for entity plugins (autonomy, controller, sensor, motion_model) +struct EntityPluginInfo { + std::string name; ///< Actual plugin name (e.g., "Straight", "MotorSchemas") + std::string type; ///< Plugin type: "autonomy", "controller", "motion_model", "sensor" + int order = 0; ///< Execution order (0, 1, 2 for multiple plugins of same type) + + /// Parameters from mission XML inline attributes (e.g., speed="21") + std::map params; + + int entity_block_id = -1; ///< Which entity block this came from + std::string entity_name; ///< uav_entity if present + std::string entity_tag; ///< tag="gen_straight" if present +}; + class MissionParse { public: bool create_log_dir(); @@ -105,14 +117,18 @@ class MissionParse { std::string log_dir(); std::string root_log_dir(); - std::map& entity_attributes(); - std::map>& entity_params(); std::map& ent_id_to_block_id(); EntityDesc_t& entity_descriptions(); + /// Returns map> for all entity blocks + const std::map>& all_entity_plugins() const; + + /// Get plugins by type, ordered by execution order + std::vector get_plugins_by_type(int entity_block_id, const std::string& type) const; + std::map& entity_tag_to_id(); bool enable_gui(); @@ -190,17 +206,18 @@ class MissionParse { std::map team_info_; - std::map entity_attributes_; std::map> entity_params_; // Key: Entity ID - // Value: XML "entity" block used to create entity - // This can be used to find the key for which entity_attributes_ maps to - // the entity XML block. + // Value: XML "entity" block ID used to create entity + // Maps runtime entity IDs to their source entity_plugins_ block ID. std::map ent_id_to_block_id_; EntityDesc_t entity_descs_; + /// Key: entity block ID, Value: map of plugin key (type:name) to EntityPluginInfo + std::map> entity_plugins_; + bool use_exact_log_path_; std::string root_log_dir_; std::string log_dir_; diff --git a/include/scrimmage/parse/MissionValidation.h b/include/scrimmage/parse/MissionValidation.h new file mode 100644 index 0000000000..1d0653e9d4 --- /dev/null +++ b/include/scrimmage/parse/MissionValidation.h @@ -0,0 +1,130 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Ethan M Boos + * @date 06 April 2026 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#ifndef INCLUDE_SCRIMMAGE_PARSE_MISSIONVALIDATION_H_ +#define INCLUDE_SCRIMMAGE_PARSE_MISSIONVALIDATION_H_ + +#include +#include +#include +#include +#include + +namespace scrimmage { + +// Maps plugin directory names to internal type names. +// Update here if plugin types are added or renamed. +inline const std::map kPluginDirToType = { + {"autonomy", "autonomy"}, + {"controller", "controller"}, + {"motion", "motion_model"}, + {"sensor", "sensor"}, + {"interaction", "entity_interaction"}, + {"metrics", "metrics"}, + {"network", "network"} +}; + +// Entity-level plugin types (attached to specific entities, not global). +inline const std::set kEntityPluginTypes = { + "autonomy", "controller", "motion_model", "sensor" +}; + +class FileSearch; +class MissionParse; + +struct ValidationError { + enum class Type { + PLUGIN_CONFIG_NOT_FOUND, + PLUGIN_LIBRARY_NOT_FOUND, + PLUGIN_LIBRARY_INVALID, + UNKNOWN_PLUGIN_TYPE + }; + + Type type; + std::string plugin_name; + std::string plugin_type; // autonomy, controller, motion_model, etc. + std::string context; // location in mission file (entity block index or "global") + std::string message; + std::vector suggestions; +}; + +struct PluginValidationInfo { + enum class Status { + unknown, + valid, + config_not_found, + }; + + Status status = Status::unknown; + std::string config_xml_path; + std::vector suggestions; +}; + +struct ValidationResult { + bool valid() const { return errors.empty(); } + std::vector errors; + std::map> available_plugins; + std::map> entity_plugin_diagnostics; +}; + +class MissionValidation { + public: + ValidationResult validate( + const std::shared_ptr& mp, + FileSearch& file_search); + + void print_errors( + const ValidationResult& result, + const std::string& mission_filename) const; + + void set_verbose(bool verbose) { verbose_ = verbose; } + + protected: + std::map> discover_plugins_by_type( + const std::string& env_var, + FileSearch& file_search); + + std::vector find_similar( + const std::string& name, + const std::set& available, + size_t max_suggestions = 3) const; + + size_t levenshtein_distance( + const std::string& s1, + const std::string& s2) const; + + private: + bool verbose_ = false; +}; + +} // namespace scrimmage + +#endif // INCLUDE_SCRIMMAGE_PARSE_MISSIONVALIDATION_H_ diff --git a/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.h b/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.h new file mode 100644 index 0000000000..5e20633d4e --- /dev/null +++ b/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.h @@ -0,0 +1,52 @@ +/*! + * @file + */ + +#ifndef INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RUNTIMEOVERRIDESPAWNER_RUNTIMEOVERRIDESPAWNER_H_ +#define INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RUNTIMEOVERRIDESPAWNER_RUNTIMEOVERRIDESPAWNER_H_ + +#include + +#include +#include + +#include "scrimmage/autonomy/Autonomy.h" + +namespace scrimmage { +namespace autonomy { + +class RuntimeOverrideSpawner : public scrimmage::Autonomy { + public: + void init(std::map& params) override; + bool step_autonomy(double t, double dt) override; + + protected: + void publish_spawn(int index, int row, int col, const Eigen::Vector3d& anchor); + + PublisherPtr pub_gen_ents_; + + // Entity template and naming + std::string entity_tag_ = "runtime_override_agent"; + std::string name_prefix_ = "runtime_override_agent"; + + // Layout configuration + int spawn_count_ = 120; + int columns_ = 12; + double x_spacing_ = 20.0; + double y_spacing_ = 20.0; + double z_spacing_ = 0.0; + double x_offset_ = 0.0; + double y_offset_ = 0.0; + double z_offset_ = 0.0; + double spawn_time_ = 0.1; + bool spawned_ = false; + + int desired_altitude_idx_ = 0; + int desired_heading_idx_ = 0; + int desired_speed_idx_ = 0; +}; + +} // namespace autonomy +} // namespace scrimmage + +#endif // INCLUDE_SCRIMMAGE_PLUGINS_AUTONOMY_RUNTIMEOVERRIDESPAWNER_RUNTIMEOVERRIDESPAWNER_H_ \ No newline at end of file diff --git a/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.xml b/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.xml new file mode 100644 index 0000000000..a88f67019e --- /dev/null +++ b/include/scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.xml @@ -0,0 +1,20 @@ + + + + RuntimeOverrideSpawner_plugin + + + runtime_override_agent + runtime_override_agent + + + 120 + 12 + 20 + 20 + 0 + 0 + 0 + 0 + 0.1 + \ No newline at end of file diff --git a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h b/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h deleted file mode 100644 index a5903cfe91..0000000000 --- a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h +++ /dev/null @@ -1,59 +0,0 @@ -/*! - * @file - * - * @section LICENSE - * - * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) - * - * This file is part of SCRIMMAGE. - * - * SCRIMMAGE is free software: you can redistribute it and/or modify it under - * the terms of the GNU Lesser General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT - * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public - * License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with SCRIMMAGE. If not, see . - * - * @author Kevin DeMarco - * @author Eric Squires - * @date 31 July 2017 - * @version 0.1.0 - * @brief Brief file description. - * @section DESCRIPTION - * A Long description goes here. - * - */ - -#ifndef INCLUDE_SCRIMMAGE_PLUGINS_METRICS_OPENAIREWARDS_OPENAIREWARDS_H_ -#define INCLUDE_SCRIMMAGE_PLUGINS_METRICS_OPENAIREWARDS_OPENAIREWARDS_H_ - -#include -#include - -#include "scrimmage/metrics/Metrics.h" - -namespace scrimmage { -namespace metrics { - -class OpenAIRewards : public scrimmage::Metrics { - public: - OpenAIRewards(); - void init(std::map& params) override; - bool step_metrics(double /*t*/, double /*dt*/) override { return true; } - void print_team_summaries() override; - void calc_team_scores() override; - - protected: - std::map params_; - std::map rewards_; -}; - -} // namespace metrics -} // namespace scrimmage -#endif // INCLUDE_SCRIMMAGE_PLUGINS_METRICS_OPENAIREWARDS_OPENAIREWARDS_H_ diff --git a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.xml b/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.xml deleted file mode 100644 index 9d954f713e..0000000000 --- a/include/scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - OpenAIRewards_plugin - - - -1.0 - - diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index 1690ebbe0a..f260d89547 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -50,6 +50,7 @@ #include "scrimmage/common/DelayedTask.h" #include "scrimmage/common/FileSearch.h" #include "scrimmage/common/Timer.h" +#include "scrimmage/entity/RuntimePluginOverrides.h" #include "scrimmage/fwd_decl.h" #include "scrimmage/proto/Shape.pb.h" #include "scrimmage/proto/Visual.pb.h" @@ -206,14 +207,29 @@ class SimControl { bool generate_entity(const int& ent_desc_id); /** - * @brief Generate an entity given the entity description ID, - * parameters, and plugin specific params. This is utilized for - * GenerateEntity publishers. + * @brief Generate an entity with parameter overrides. + * + * @param ent_desc_id Entity block ID from the mission file. + * @param params Overrides for entity properties (position, color, etc.). + * Plugin names (autonomy, controller, motion_model, sensor) + * cannot be overridden and are ignored if present. + */ + bool generate_entity( + const int& ent_desc_id, + std::map& params); + + /** + * @brief Generate an entity with entity and plugin parameter overrides. + * + * SimControl receives GenerateEntity protobuf messages via pubsub and uses + * this method to spawn entities with runtime parameter overrides parsed from + * those messages. Validates overrides against mission/plugin configs before + * passing them to Entity::init(). */ bool generate_entity( const int& ent_desc_id, std::map& params, - AttributeMap& plugin_attr_map); + const RuntimePluginOverrides& runtime_plugin_overrides); /// @brief Get the pointer to the MissionParser instance. MissionParsePtr mp(); diff --git a/missions/rlconsensus.xml b/missions/rlconsensus.xml deleted file mode 100644 index c41f9c02ae..0000000000 --- a/missions/rlconsensus.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - 50051 - localhost - - time - - 1 - 1000 - - 191 191 191 - 10 - - false - nothing - false - false - - ~/.scrimmage/logs - - 35.721025 - -120.767925 - 300 - false - 10 - - - - - - - 3361078855 - - - 1 - 1 - 1 - - 1 - Sphere - SingleIntegrator - SingleIntegratorControllerSimple - RLConsensusSensor - RLConsensus - 0 - 0 - - - - 2 - 0 - 5 - 77 77 255 - - diff --git a/missions/rlsimple.xml b/missions/rlsimple.xml deleted file mode 100644 index 6b620b2b66..0000000000 --- a/missions/rlsimple.xml +++ /dev/null @@ -1,67 +0,0 @@ - - - - - 50051 - localhost - - time - - 1 - 1000 - - 191 191 191 - 10 - - false - summary - false - false - - ~/.scrimmage/logs - - 35.721025 - -120.767925 - 300 - false - 10 - - - - - - - 3361078855 - GlobalNetwork - OpenAIRewards - - - 1 - 1 - 1 - - 1 - Sphere - SingleIntegrator - SingleIntegratorControllerSimple - RLSimpleSensor - RLSimple - 0 - 0 - - - - 0 - 77 77 255 - - diff --git a/missions/test-duplicate-invalid-plugin.xml b/missions/test-duplicate-invalid-plugin.xml new file mode 100644 index 0000000000..9e20eb87a5 --- /dev/null +++ b/missions/test-duplicate-invalid-plugin.xml @@ -0,0 +1,41 @@ + + + + + + time + 10 + 1000 + mcmillan + ~/.scrimmage/logs + false + 35.721025 + -120.767925 + 300 + + + invalid_plugin_duplicate_test + 1 + 77 77 255 + 1 + 1 + + 0 + 0 + 200 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + Straigt + Straigt + + + \ No newline at end of file diff --git a/missions/test-duplicate-plugins.xml b/missions/test-duplicate-plugins.xml new file mode 100644 index 0000000000..3e9f787818 --- /dev/null +++ b/missions/test-duplicate-plugins.xml @@ -0,0 +1,45 @@ + + + + + + time + 10 + 1000 + mcmillan + ~/.scrimmage/logs + false + 35.721025 + -120.767925 + 300 + SimpleCollision + 12345 + + + + + duplicate_autonomy_test + 1 + 77 77 255 + 1 + 1 + + 0 + 0 + 200 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + Straight + Straight + + + diff --git a/missions/test-invalid-plugin.xml b/missions/test-invalid-plugin.xml new file mode 100644 index 0000000000..5372fdc767 --- /dev/null +++ b/missions/test-invalid-plugin.xml @@ -0,0 +1,44 @@ + + + + + + time + 10 + 1000 + mcmillan + ~/.scrimmage/logs + false + 35.721025 + -120.767925 + 300 + SimpleCollision + 12345 + + + + invalid_plugin_test + 1 + 77 77 255 + 1 + 1 + + 0 + 0 + 200 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + + Straigt + + + diff --git a/missions/test-plugin-metadata.xml b/missions/test-plugin-metadata.xml new file mode 100644 index 0000000000..cf4a7efe62 --- /dev/null +++ b/missions/test-plugin-metadata.xml @@ -0,0 +1,151 @@ + + + + + + + 50051 + localhost + + time + + 10 + 1000 + + mcmillan + 191 191 191 + 10 + + false + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs + false + + 35.721025 + -120.767925 + 300 + + SimpleCollision + LocalNetwork + + 12345 + + + + + multi_autonomy_entity + 1 + 77 77 255 + 1 + 1 + + 0 + 0 + 200 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + + Straight + MotorSchemas + + + + + + params_test_entity + 1 + 255 128 0 + 1 + 1 + + 100 + 0 + 200 + 90 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + Straight + + + + + + named_and_tagged + 2 + 255 0 0 + 1 + 1 + + 200 + 0 + 200 + 180 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-red + + Straight + + + + + + sensor_test_entity + 1 + 0 255 0 + 1 + 1 + + 300 + 0 + 200 + 270 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + RayTrace + ContactBlobCamera + + Straight + + + + + + 1 + 128 128 128 + 1 + 1 + + 400 + 0 + 200 + 0 + + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + Straight + + + diff --git a/missions/test/test_entity_common_motion_override.xml b/missions/test/test_entity_common_motion_override.xml new file mode 100644 index 0000000000..ae9834a92c --- /dev/null +++ b/missions/test/test_entity_common_motion_override.xml @@ -0,0 +1,41 @@ + + + + + + time + 10 + 1000 + mcmillan + ~/.scrimmage/logs + false + 35.721025 + -120.767925 + 300 + + + 1 + 77 77 255 + 1 + 1 + 0 + 0 + 200 + 0 + zephyr-blue + + SimpleAircraftControllerPID + SimpleAircraft + Straight + + + + entity_common_motion_override + Ballistic + + \ No newline at end of file diff --git a/missions/test/test_entity_common_plugins.xml b/missions/test/test_entity_common_plugins.xml new file mode 100644 index 0000000000..cc570795ee --- /dev/null +++ b/missions/test/test_entity_common_plugins.xml @@ -0,0 +1,42 @@ + + + + + + time + 10 + 1000 + mcmillan + ~/.scrimmage/logs + false + 35.721025 + -120.767925 + 300 + + + 1 + 77 77 255 + 1 + 1 + 0 + 0 + 200 + 0 + zephyr-blue + + Straight + SimpleAircraftControllerPID + SimpleAircraft + NoisyState + + + + entity_common_stack + Straight + + \ No newline at end of file diff --git a/missions/test/test_generate_entity_runtime_override_stress.xml b/missions/test/test_generate_entity_runtime_override_stress.xml new file mode 100644 index 0000000000..a50a78f8c4 --- /dev/null +++ b/missions/test/test_generate_entity_runtime_override_stress.xml @@ -0,0 +1,73 @@ + + + + + + + false + time + all + false + true + + SimpleCollisionMetrics + + ~/.scrimmage/logs/test_runtime_override_stress + true + + 35.721025 + -120.767925 + 300 + + GlobalNetwork + 2147483648 + + + runtime_override_spawner + 99 + 255 255 0 + 1 + 1 + 1 + + -200 + -200 + 100 + 0 + + RuntimeOverrideSpawner + UnicyclePID + Unicycle3D + zephyr-blue + + + + 1 + 77 77 255 + 0 + 1 + 1 + + 0 + 0 + 100 + 0 + + APITester + UnicyclePID + Unicycle3D + zephyr-blue + + + \ No newline at end of file diff --git a/missions/test/test_generate_entity_runtime_overrides.xml b/missions/test/test_generate_entity_runtime_overrides.xml new file mode 100644 index 0000000000..9fefdcea4c --- /dev/null +++ b/missions/test/test_generate_entity_runtime_overrides.xml @@ -0,0 +1,47 @@ + + + + + + + false + time + all + false + + SimpleCollisionMetrics + + ~/.scrimmage/logs/test_runtime_overrides + true + + 35.721025 + -120.767925 + 300 + + GlobalNetwork + 2147483648 + + + 1 + 77 77 255 + 0 + 1 + 1 + + 0 + 0 + 100 + 0 + + APITester + UnicyclePID + Unicycle3D + zephyr-blue + + + \ No newline at end of file diff --git a/missions/test/test_mission_include/test_mission_parse_mission_include.xml b/missions/test/test_mission_include/test_mission_parse_mission_include.xml index 9e0d0fd9c3..67b0ff70ca 100644 --- a/missions/test/test_mission_include/test_mission_parse_mission_include.xml +++ b/missions/test/test_mission_include/test_mission_parse_mission_include.xml @@ -62,6 +62,7 @@ to LocalNetwork's csv filename attribute 20 LocalNetwork +SphereNetwork 2147483648 diff --git a/missions/test/test_mission_parse_mission.xml b/missions/test/test_mission_parse_mission.xml index 1eb6ddb69b..992284a3de 100644 --- a/missions/test/test_mission_parse_mission.xml +++ b/missions/test/test_mission_parse_mission.xml @@ -61,6 +61,7 @@ to LocalNetwork's csv filename attribute 20 LocalNetwork +SphereNetwork diff --git a/msgs/Event.proto b/msgs/Event.proto index 14528f0fe0..b94918beea 100644 --- a/msgs/Event.proto +++ b/msgs/Event.proto @@ -40,10 +40,11 @@ message KeyValuePair { string value = 2; } -message KeyValueAttr { +message PluginParamOverride { string plugin_type = 1; - string tag_name = 2; - string tag_value = 3; + uint32 plugin_index = 2; + repeated KeyValuePair params = 3; + optional string plugin_name = 4; // name will resolve to a plugin_index } message GenerateEntity { @@ -51,5 +52,5 @@ message GenerateEntity { scrimmage_proto.State state = 2; repeated KeyValuePair entity_param = 3; int32 entity_id = 4; - repeated KeyValueAttr plugin_param = 5; + repeated PluginParamOverride plugin_override = 5; } diff --git a/share/scrimmage/main.cpp b/share/scrimmage/main.cpp index 97809d36fa..f859b9da57 100644 --- a/share/scrimmage/main.cpp +++ b/share/scrimmage/main.cpp @@ -163,7 +163,6 @@ int main(int argc, char* argv[]) { simcontrol.mp()->set_job_number(job_id); simcontrol.mp()->set_overrides(overrides.str()); if (not simcontrol.init(mission_file)) { - cout << "Failed to initialize SimControl with mission file: " << mission_file << endl; return -1; } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 16a17a5e06..0533d70013 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -16,12 +16,13 @@ set(SRCS common/Shape.cpp entity/Contact.cpp entity/Entity.cpp entity/External.cpp entity/EntityPlugin.cpp + entity/RuntimePluginOverrides.cpp log/FrameUpdateClient.cpp log/Log.cpp log/Logger.cpp math/Angles.cpp math/Quaternion.cpp math/State.cpp math/StateWithCovariance.cpp metrics/Metrics.cpp network/Interface.cpp network/ScrimmageServiceImpl.cpp - parse/ConfigParse.cpp parse/MissionParse.cpp parse/ParseUtils.cpp + parse/ConfigParse.cpp parse/MissionParse.cpp parse/MissionValidation.cpp parse/ParseUtils.cpp #parse/XMLParser/RapidXMLParser.cpp plugin_manager/MotionModel.cpp plugin_manager/Plugin.cpp plugin_manager/PluginManager.cpp diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index f73d48bfb3..04324b5d42 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -88,8 +88,8 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { int id = init_params.id; int ent_desc_id = init_params.ent_desc_id; std::map& info = init_params.info; - AttributeMap& overrides = init_params.overrides; std::set& plugin_tags = init_params.plugin_tags; + const RuntimePluginOverrides& runtime_plugin_overrides = init_params.runtime_plugin_overrides; std::function&)> param_override_func = init_params.param_override_func; int debug_level = init_params.debug_level; @@ -107,7 +107,7 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { mp_ = std::make_shared(); } else { mp_ = mp; - parse_visual(info, mp_, overrides["visual_model"]); + parse_visual(info, mp_); } if (info.count("health") > 0) { @@ -159,20 +159,20 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { //////////////////////////////////////////////////////////// // sensor //////////////////////////////////////////////////////////// - // The MissionParser appends the order number to the sensor (e.g., sensor0, - // sensor1, etc.) - int sensor_ct = 0; - std::string sensor_order_name = std::string("sensor") + std::to_string(sensor_ct); - - while (info.count(sensor_order_name) > 0) { + auto sensor_plugins = mp_->get_plugins_by_type(ent_desc_id, "sensor"); + for (const auto& plugin_info : sensor_plugins) { ConfigParse config_parse; - std::string sensor_name = info[sensor_order_name]; + std::string sensor_name = plugin_info.name; + + std::map sensor_overrides = + resolve_plugin_params(plugin_info, runtime_plugin_overrides); + PluginStatus status = plugin_manager_->make_plugin( "scrimmage::Sensor", sensor_name, *file_search_, config_parse, - overrides[sensor_order_name], + sensor_overrides, plugin_tags); if (status.status == PluginStatus::cast_failed) { LOG_ERROR("Failed to open sensor plugin: " << sensor_name); @@ -185,16 +185,16 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { // Get sensor's offset from entity origin std::vector tf_xyz = {0.0, 0.0, 0.0}; - auto it_xyz = overrides[sensor_order_name].find("xyz"); - if (it_xyz != overrides[sensor_order_name].end()) { + auto it_xyz = sensor_overrides.find("xyz"); + if (it_xyz != sensor_overrides.end()) { str2container(it_xyz->second, " ", tf_xyz, 3); } sensor->transform()->pos() << tf_xyz[0], tf_xyz[1], tf_xyz[2]; // Get sensor's orientation relative to entity's coordinate frame std::vector tf_rpy = {0.0, 0.0, 0.0}; - auto it_rpy = overrides[sensor_order_name].find("rpy"); - if (it_rpy != overrides[sensor_order_name].end()) { + auto it_rpy = sensor_overrides.find("rpy"); + if (it_rpy != sensor_overrides.end()) { str2container(it_rpy->second, " ", tf_rpy, 3); } sensor->transform()->quat().set( @@ -217,7 +217,7 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { sensor->set_loop_rate(loop_rate); } - std::string given_name = sensor_name + std::to_string(sensor_ct); + std::string given_name = sensor_name + std::to_string(plugin_info.order); sensor->set_name(given_name); if (debug_level > 1) { @@ -236,7 +236,6 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { } sensors_[given_name] = sensor; } - sensor_order_name = std::string("sensor") + std::to_string(++sensor_ct); } //////////////////////////////////////////////////////////// @@ -245,20 +244,26 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { bool use_gpu_motion_model = gpu_motion_model != nullptr; bool init_empty_motion_model = true; // Still init a dummy motion model when using a gpu one. // Otherwise program may segfault during execution. - if (info.count("motion_model") > 0 && !use_gpu_motion_model) { + auto motion_plugins = mp_->get_plugins_by_type(ent_desc_id, "motion_model"); + if (!motion_plugins.empty() && !use_gpu_motion_model) { + // MissionParse enforces singleton motion_model precedence, so front() is the + // resolved winner after entity-local overrides have replaced any inherited one. + const auto& motion_info = motion_plugins.front(); ConfigParse config_parse; + std::map motion_overrides = + resolve_plugin_params(motion_info, runtime_plugin_overrides); PluginStatus status = plugin_manager_->make_plugin( "scrimmage::MotionModel", - info["motion_model"], + motion_info.name, *file_search_, config_parse, - overrides["motion_model"], + motion_overrides, plugin_tags); if (status.status == PluginStatus::cast_failed) { - LOG_ERROR("Failed to open motion model plugin: " << info["motion_model"]); + LOG_ERROR("Failed to open motion model plugin: " << motion_info.name); return false; } else if (status.status == PluginStatus::parse_failed) { - LOG_ERROR("Failed to parse motion model plugin config: " << info["motion_model"]); + LOG_ERROR("Failed to parse motion model plugin config: " << motion_info.name); return false; } else if (status.status == PluginStatus::loaded) { // We have created a valid motion model @@ -272,21 +277,21 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { motion_model_->set_id_to_team_map(id_to_team_map); motion_model_->set_id_to_ent_map(id_to_ent_map); motion_model_->set_param_server(param_server_); - motion_model_->set_name(info["motion_model"]); + motion_model_->set_name(motion_info.name); param_override_func(config_parse.params()); if (debug_level > 1) { LOG_INFO("--------------------------------"); - LOG_INFO("Motion plugin params: " << info["motion_model"]); + LOG_INFO("Motion plugin params: " << motion_info.name); LOG_INFO(config_parse); } try { motion_model_->init(info, config_parse.params()); } catch (const std::exception& e) { - LOG_ERROR("MotionModel plugin '" << info["motion_model"] << "' threw exception during init: " << e.what()); + LOG_ERROR("MotionModel plugin '" << motion_info.name << "' threw exception during init: " << e.what()); return false; } catch (...) { - LOG_ERROR("MotionModel plugin '" << info["motion_model"] << "' threw unknown exception during init"); + LOG_ERROR("MotionModel plugin '" << motion_info.name << "' threw unknown exception during init"); return false; } } @@ -308,41 +313,29 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { //////////////////////////////////////////////////////////// // controller //////////////////////////////////////////////////////////// - // Create a list of controller names (from XML top-down order) - std::list controller_names; - int controller_ct = 0; - std::string controller_name = std::string("controller") + std::to_string(controller_ct); - bool valid_controller_name = true; - do { - if (info.count(controller_name) > 0) { - controller_names.push_back(controller_name); - } else { - valid_controller_name = false; - } - controller_name = std::string("controller") + std::to_string(++controller_ct); - } while (valid_controller_name); + auto controller_plugins = mp_->get_plugins_by_type(ent_desc_id, "controller"); - // Reverse iterate over controllers, so that the VariableIO can be setup - // correctly. Last controller connects to motion model, second to last - // controller connects to the last controller. - for (std::list::reverse_iterator rit = controller_names.rbegin(); - rit != controller_names.rend(); - ++rit) { - std::string controller_name = *rit; + // Build the controller chain from back to front. The last controller in + // XML order feeds the motion model, so it must be created first. Each + // earlier controller then connects to the controller that was just added. + for (auto rit = controller_plugins.rbegin(); rit != controller_plugins.rend(); ++rit) { + const auto& controller_info = *rit; ConfigParse config_parse; + std::map controller_overrides = + resolve_plugin_params(controller_info, runtime_plugin_overrides); PluginStatus status = plugin_manager_->make_plugin( "scrimmage::Controller", - info[controller_name], + controller_info.name, *file_search_, config_parse, - overrides[controller_name], + controller_overrides, plugin_tags); if (status.status == PluginStatus::cast_failed) { - LOG_ERROR("Failed to open controller plugin: " << controller_name); + LOG_ERROR("Failed to open controller plugin: " << controller_info.name); return false; } else if (status.status == PluginStatus::parse_failed) { - LOG_ERROR("Failed to parse controller plugin config: " << info[controller_name]); + LOG_ERROR("Failed to parse controller plugin config: " << controller_info.name); return false; } else if (status.status == PluginStatus::loaded) { ControllerPtr controller = status.plugin; @@ -353,7 +346,7 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { controller->set_id_to_ent_map(id_to_ent_map); controller->set_param_server(param_server_); controller->set_pubsub(pubsub_); - controller->set_name(info[controller_name]); + controller->set_name(controller_info.name); param_override_func(config_parse.params()); // get loop rate from plugin's params @@ -384,16 +377,16 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { // Initialize this controller. if (debug_level > 1) { LOG_INFO("--------------------------------"); - LOG_INFO("Controller plugin params: " << info[controller_name]); + LOG_INFO("Controller plugin params: " << controller_info.name); LOG_INFO(config_parse); } try { controller->init(config_parse.params()); } catch (const std::exception& e) { - LOG_ERROR("Controller plugin '" << info[controller_name] << "' threw exception during init: " << e.what()); + LOG_ERROR("Controller plugin '" << controller_info.name << "' threw exception during init: " << e.what()); return false; } catch (...) { - LOG_ERROR("Controller plugin '" << info[controller_name] << "' threw unknown exception during init"); + LOG_ERROR("Controller plugin '" << controller_info.name << "' threw unknown exception during init"); return false; } @@ -447,21 +440,14 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { //////////////////////////////////////////////////////////// // autonomy //////////////////////////////////////////////////////////// - // Create a list of autonomy names - std::list autonomy_names; - int autonomy_ct = 0; - std::string autonomy_name = std::string("autonomy") + std::to_string(autonomy_ct); - while (info.count(autonomy_name) > 0) { - autonomy_names.push_back(autonomy_name); - autonomy_name = std::string("autonomy") + std::to_string(++autonomy_ct); - } + auto autonomy_plugins = mp_->get_plugins_by_type(ent_desc_id, "autonomy"); - // Create the autonomy plugins from the autonomy_names list. - for (auto autonomy_name : autonomy_names) { + // Create the autonomy plugins + for (const auto& autonomy_info : autonomy_plugins) { auto autonomy = make_autonomy( - info[autonomy_name], + autonomy_info.name, plugin_manager_, - overrides[autonomy_name], + resolve_plugin_params(autonomy_info, runtime_plugin_overrides), parent, state_belief_, id_to_team_map, @@ -526,12 +512,12 @@ bool Entity::init(const SimUtilsInfo& sim_info, EntityInitParams init_params) { bool Entity::parse_visual( std::map& info, - MissionParsePtr mp, - std::map& overrides) { + MissionParsePtr mp) { visual_->set_id(id_.id()); visual_->set_opacity(1.0); ConfigParse cv_parse; + std::map model_overrides; bool mesh_found, texture_found; auto it = info.find("visual_model"); if (it == info.end()) { @@ -542,7 +528,7 @@ bool Entity::parse_visual( it->second, cv_parse, *file_search_, - overrides, + model_overrides, visual_, mesh_found, texture_found); diff --git a/src/entity/External.cpp b/src/entity/External.cpp index aa0b505f9d..1348456702 100644 --- a/src/entity/External.cpp +++ b/src/entity/External.cpp @@ -212,17 +212,7 @@ bool External::create_entity( std::map info = mp_->entity_descriptions()[it_name_id->second]; - AttributeMap& attr_map = mp_->entity_attributes()[it_name_id->second]; - // bool ent_success = - // entity_->init( - // attr_map, info, id_to_team_map_, id_to_ent_map_, contacts, mp_, - // mp_->projection(), entity_id, - // it_name_id->second, plugin_manager_, file_search, rtree, pubsub_, - // printer_, time_, param_server_, global_services_, plugin_tags, - // param_override_func, debug_level); - // EntityInitParams init_params; - init_params.overrides = attr_map; init_params.id = entity_id; init_params.info = info; init_params.ent_desc_id = it_name_id->second; diff --git a/src/entity/RuntimePluginOverrides.cpp b/src/entity/RuntimePluginOverrides.cpp new file mode 100644 index 0000000000..379e6fedb9 --- /dev/null +++ b/src/entity/RuntimePluginOverrides.cpp @@ -0,0 +1,314 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Ethan M Boos + * @date 13 April 2026 + * @version 0.1.0 + * @brief Implementation of runtime plugin override utilities. + * @section DESCRIPTION + * Parsing and validation logic for per-spawn plugin parameter overrides + * passed through the GenerateEntity mechanism. + * + */ + +#include "scrimmage/entity/RuntimePluginOverrides.h" + +#include +#include +#include +#include + +#include "scrimmage/common/FileSearch.h" +#include "scrimmage/log/Logger.h" +#include "scrimmage/msgs/Event.pb.h" +#include "scrimmage/parse/ConfigParse.h" +#include "scrimmage/parse/MissionParse.h" +#include "scrimmage/parse/MissionValidation.h" +#include "scrimmage/parse/ParseUtils.h" + +namespace scrimmage { + +namespace { + +// Strict runtime override validation derives allowed keys from +// ConfigParse::params(). That map includes parser metadata and plugin loading +// fields, so strip those out before treating the remaining names as user +// overridable runtime parameters. +const std::set kIgnoredPluginConfigParams = { + "XML_DIR", + "XML_FILENAME", + "library" +}; + +// Sensor placement keys that Entity.cpp reads to mount sensors on the vehicle. +// Unlike behavior params (e.g., max_range, fov), these are NOT declared in +// plugin XML files like RayTrace.xml — they're construction-time placement +// params that Entity.cpp handles specially. If a mission template omits them +// (using default placement), they won't appear in either: +// - Source 1 (plugin XML config), or +// - Source 2 (mission inline attributes) +// But you may still want to override them at runtime (e.g., spawn one drone +// with sensor pointing left, another pointing right). So we hardcode them as +// always-allowed for sensors. +const std::set kSensorRuntimeOverrideParams = { + "rpy", + "xyz" +}; + +} // namespace + +bool is_plugin_name_override_key(const std::string& key) { + if (kEntityPluginTypes.count(key) > 0) { + return true; + } + + const size_t suffix_end = key.find_last_not_of("0123456789"); + if (suffix_end == std::string::npos) { + return false; + } + + return kEntityPluginTypes.count(key.substr(0, suffix_end + 1)) > 0; +} + +// Resolve plugin_name to plugin_index within the given plugin type for an entity. +// Returns -1 if name not found, -2 if name matches multiple plugins. +static int resolve_plugin_name_to_index( + MissionParsePtr mp, + int ent_desc_id, + const std::string& plugin_type, + const std::string& plugin_name) { + const auto plugins = mp->get_plugins_by_type(ent_desc_id, plugin_type); + int found_index = -1; + for (size_t i = 0; i < plugins.size(); ++i) { + if (plugins[i].name == plugin_name) { + if (found_index >= 0) { + return -2; // Duplicate name found + } + found_index = static_cast(i); + } + } + return found_index; +} + +bool parse_runtime_plugin_overrides( + const scrimmage_msgs::GenerateEntity& data, + MissionParsePtr mp, + int ent_desc_id, + RuntimePluginOverrides& runtime_plugin_overrides) { + for (int i = 0; i < data.plugin_override_size(); ++i) { + const auto& proto_override = data.plugin_override(i); + if (kEntityPluginTypes.count(proto_override.plugin_type()) == 0) { + LOG_ERROR("GenerateEntity: Unknown plugin_type '" << proto_override.plugin_type() + << "'. Expected one of autonomy, controller, motion_model, sensor."); + return false; + } + + // Resolve plugin_name to plugin_index if specified + int resolved_index = static_cast(proto_override.plugin_index()); + if (proto_override.has_plugin_name()) { + resolved_index = resolve_plugin_name_to_index( + mp, ent_desc_id, proto_override.plugin_type(), proto_override.plugin_name()); + if (resolved_index == -1) { + LOG_ERROR("GenerateEntity: Plugin name '" << proto_override.plugin_name() + << "' not found in " << proto_override.plugin_type() + << " plugins for entity block " << ent_desc_id << "."); + return false; + } + if (resolved_index == -2) { + LOG_ERROR("GenerateEntity: Plugin name '" << proto_override.plugin_name() + << "' matches multiple " << proto_override.plugin_type() + << " plugins. Use plugin_index to disambiguate."); + return false; + } + } + + auto& instance_overrides = + runtime_plugin_overrides[proto_override.plugin_type()][resolved_index]; + if (!instance_overrides.empty()) { + LOG_ERROR("GenerateEntity: Duplicate plugin_override entry for " + << proto_override.plugin_type() << "[" << resolved_index + << "]. Merge parameters into a single override block."); + return false; + } + + for (int j = 0; j < proto_override.params_size(); ++j) { + const auto& param = proto_override.params(j); + if (instance_overrides.count(param.key()) > 0) { + LOG_ERROR("GenerateEntity: Duplicate runtime plugin parameter '" << param.key() + << "' for " << proto_override.plugin_type() << "[" + << resolved_index << "]."); + return false; + } + instance_overrides[param.key()] = param.value(); + } + } + + return true; +} + +// Builds the set of parameter names that are valid for runtime overrides. +// This combines two sources: +// 1. Params declared in the plugin's XML config file (e.g., Straight.xml) +// These are the defaults the plugin author defined. Cached because the +// plugin XML doesn't change, and parsing it repeatedly is expensive. +// 2. Params set inline in the mission file (e.g., ) +// These are entity-specific and come from plugin_info.params. Not cached +// because they vary per entity block. +// The union of both sets forms the allowed runtime override keys. +bool get_allowed_runtime_plugin_param_names( + const EntityPluginInfo& plugin_info, + FileSearch& file_search, + std::set& allowed_param_names) { + + // --- Source 1: Plugin XML config params (cached) --- + // These are params declared in the plugin's own XML file, like: + // include/scrimmage/plugins/autonomy/Straight/Straight.xml + // They represent what the plugin implementation accepts, regardless of + // whether the mission file mentions them. + static std::map> cached_param_names; + static std::mutex cache_mutex; + + const std::string cache_key = plugin_info.type + ":" + plugin_info.name; + + // Check cache first to avoid re-parsing the same plugin XML repeatedly. + { + std::lock_guard lock(cache_mutex); + auto cache_it = cached_param_names.find(cache_key); + if (cache_it != cached_param_names.end()) { + allowed_param_names = cache_it->second; + } + } + + // Cache miss: parse the plugin XML to extract declared param names. + if (allowed_param_names.empty()) { + ConfigParse config_parse; + config_parse.set_required("library"); + const std::map empty_overrides; + if (!config_parse.parse(empty_overrides, plugin_info.name, "SCRIMMAGE_PLUGIN_PATH", file_search)) { + LOG_ERROR("GenerateEntity: Failed to inspect plugin config for '" << plugin_info.name + << "' while validating runtime overrides."); + return false; + } + + for (const auto& [key, value] : config_parse.params()) { + static_cast(value); + if (kIgnoredPluginConfigParams.count(key) == 0) { + allowed_param_names.insert(key); + } + } + + std::lock_guard lock(cache_mutex); + cached_param_names[cache_key] = allowed_param_names; + } + + // --- Source 2: Mission file inline params (not cached) --- + // These are params written inline in the mission XML, like: + // Straight + // They vary per entity block, so we add them on every call. + for (const auto& [key, value] : plugin_info.params) { + static_cast(value); + allowed_param_names.insert(key); + } + + // Sensors also accept transform keys (rpy, xyz) for placement overrides. + if (plugin_info.type == "sensor") { + allowed_param_names.insert(kSensorRuntimeOverrideParams.begin(), kSensorRuntimeOverrideParams.end()); + } + + return true; +} + +bool validate_runtime_plugin_overrides( + MissionParsePtr mp, + FileSearchPtr file_search, + int ent_desc_id, + const RuntimePluginOverrides& runtime_plugin_overrides) { + const bool strict_runtime_plugin_params = + get("strict_runtime_plugin_params", mp->params(), true); + + for (const auto& [plugin_type, instance_overrides] : runtime_plugin_overrides) { + if (kEntityPluginTypes.count(plugin_type) == 0) { + LOG_ERROR("GenerateEntity: Unknown runtime override plugin type '" << plugin_type << "'."); + return false; + } + + const auto plugins = mp->get_plugins_by_type(ent_desc_id, plugin_type); + for (const auto& [plugin_index, param_overrides] : instance_overrides) { + if (plugin_index >= static_cast(plugins.size())) { + LOG_ERROR("GenerateEntity: Entity block " << ent_desc_id + << " does not define " << plugin_type << "[" << plugin_index + << "] for runtime overrides."); + return false; + } + + if (param_overrides.empty()) { + LOG_WARN("GenerateEntity: Empty runtime override for " << plugin_type << "[" + << plugin_index << "] ignored."); + } + + if (!strict_runtime_plugin_params) { + continue; + } + + std::set allowed_param_names; + if (!get_allowed_runtime_plugin_param_names(plugins[plugin_index], *file_search, allowed_param_names)) { + return false; + } + + for (const auto& [param_name, param_value] : param_overrides) { + static_cast(param_value); + if (allowed_param_names.count(param_name) == 0) { + LOG_ERROR("GenerateEntity: Runtime override key '" << param_name << "' is not declared for " + << plugin_type << "[" << plugin_index << "] plugin '" + << plugins[plugin_index].name << "'. " + << "Set strict_runtime_plugin_params=false in the mission file to allow unknown keys."); + return false; + } + } + } + } + + return true; +} + +std::map resolve_plugin_params( + const EntityPluginInfo& plugin_info, + const RuntimePluginOverrides& runtime_plugin_overrides) { + std::map resolved_params = plugin_info.params; + + auto type_it = runtime_plugin_overrides.find(plugin_info.type); + if (type_it == runtime_plugin_overrides.end()) { + return resolved_params; + } + + auto order_it = type_it->second.find(plugin_info.order); + if (order_it == type_it->second.end()) { + return resolved_params; + } + + for (const auto& [key, value] : order_it->second) { + resolved_params[key] = value; + } + + return resolved_params; +} + +} // namespace scrimmage diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index fb6fdc4703..d9886c280b 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -33,22 +33,20 @@ #include "scrimmage/parse/MissionParse.h" #include "scrimmage/common/FileSearch.h" -#include "scrimmage/common/Utilities.h" #include "scrimmage/log/Logger.h" #include "scrimmage/parse/ConfigParse.h" #include "scrimmage/parse/ParseUtils.h" #include "scrimmage/parse/XMLParser/RapidXMLParser.h" -#include "scrimmage/parse/XMLParser/XMLParser.h" #include "scrimmage/proto/ProtoConversions.h" #if ENABLE_LIBXML2_PARSER #include "scrimmage/parse/XMLParser/LibXML2Parser.h" #endif +#include #include #include //NOLINT #include -#include #include #include @@ -283,7 +281,8 @@ bool MissionParse::parse_mission() { auto parse_tags = [&](const std::string& tagname, std::list& tag_list) { for (auto node = runscript_node.first_node(tagname); node.is_valid(); node = node.next_sibling(tagname)) { - // If a "name" is specified, use this name + // For global plugins, the optional name= attribute is an instance alias. + // The actual plugin implementation name still comes from the tag value. auto attr = node.first_attribute("name"); std::string name = (!attr.is_valid()) ? node.value() : attr.value(); tag_list.push_back(name); @@ -327,6 +326,8 @@ bool MissionParse::parse_mission() { std::string nm4 = (nm == "network") ? name : nm; std::string nm5 = (nm == "gpu_kernel") ? name : nm; + // Preserve the implementation name separately so validation and plugin loading + // can resolve aliases like SphereNetwork. attributes_[nm2]["ORIGINAL_PLUGIN_NAME"] = node.value(); attributes_[nm3]["ORIGINAL_PLUGIN_NAME"] = node.value(); attributes_[nm4]["ORIGINAL_PLUGIN_NAME"] = node.value(); @@ -433,10 +434,33 @@ bool MissionParse::parse_mission() { int ent_desc_id = 0; int team_id_err = 0; // only used when team_id is not set "warn condition" - // common block name -> - std::map entity_common_attributes; + // common block name -> inherited plugin metadata parsed from + std::map> entity_common_plugins; std::map> entity_common; std::map> orders; + auto insert_entity_plugin = [&](int entity_block_id, EntityPluginInfo plugin_info) { + plugin_info.entity_block_id = entity_block_id; + + auto& plugins_map = entity_plugins_[entity_block_id]; + if (plugin_info.type == "motion_model") { + // Each entity can only have one motion model. If entity_common defines one + // and the entity block also defines one, the entity's takes precedence. + for (auto it = plugins_map.begin(); it != plugins_map.end();) { + if (it->second.type == "motion_model") { + it = plugins_map.erase(it); + } else { + ++it; + } + } + } + + std::string plugin_key = plugin_info.type + ":" + plugin_info.name; + if (plugins_map.count(plugin_key) > 0) { + plugin_key += ":" + std::to_string(plugin_info.order); + } + plugins_map[plugin_key] = std::move(plugin_info); + }; + for (auto script_node = runscript_node.first_node("entity_common"); script_node.is_valid(); script_node = script_node.next_sibling("entity_common")) { std::map script_info; @@ -452,35 +476,58 @@ bool MissionParse::parse_mission() { for (auto node = script_node.first_node(); node.is_valid(); node = node.next_sibling()) { std::string node_name = node.name(); + std::string original_type = node_name; + bool is_plugin = false; + int plugin_order = 0; if (node_name == "name") { continue; } if (node_name == "autonomy") { - node_name += std::to_string(orders[nm]["autonomy"]++); + is_plugin = true; + plugin_order = orders[nm]["autonomy"]++; + node_name += std::to_string(plugin_order); } else if (node_name == "controller") { - node_name += std::to_string(orders[nm]["controller"]++); + is_plugin = true; + plugin_order = orders[nm]["controller"]++; + node_name += std::to_string(plugin_order); } else if (node_name == "sensor") { - node_name += std::to_string(orders[nm]["sensor"]++); + is_plugin = true; + plugin_order = orders[nm]["sensor"]++; + node_name += std::to_string(plugin_order); + } else if (node_name == "motion_model") { + is_plugin = true; } + std::string node_value = trim(node.value()); + std::map node_params; + // Loop through each node's attributes: for (auto attr = node.first_attribute(); attr.is_valid(); attr = attr.next()) { const std::string attr_name = attr.name(); if (attr_name == "param_common") { for (auto& kv : param_common[attr.value()]) { - entity_common_attributes[nm][node_name][kv.first] = kv.second; + node_params[kv.first] = kv.second; } } else { - entity_common_attributes[nm][node_name][attr_name] = attr.value(); + node_params[attr_name] = attr.value(); } } + if (is_plugin && !node_value.empty()) { + EntityPluginInfo plugin_info; + plugin_info.name = node_value; + plugin_info.type = original_type; + plugin_info.order = plugin_order; + plugin_info.params = node_params; + entity_common_plugins[nm].push_back(plugin_info); + } + if (script_info.count(node_name) > 0 && node_name.compare("team_id")) { LOG_WARN("Warning: entity contains multiple tags for \"" << node_name << "\""); } - script_info[node_name] = node.value(); + script_info[node_name] = node_value; } entity_common[nm] = script_info; @@ -501,10 +548,18 @@ bool MissionParse::parse_mission() { LOG_WARN("entity_common block referenced without definition"); } else { script_info = it->second; - entity_attributes_[ent_desc_id] = entity_common_attributes[nm]; autonomy_order = orders[nm]["autonomy"]; controller_order = orders[nm]["controller"]; sensor_order = orders[nm]["sensor"]; + + // Copy plugins from the referenced entity_common block before reading this + // entity's own plugin tags. autonomy/controller/sensor entries keep both the + // copied and local instances, with the local ones getting later order values. + // A local motion_model replaces the copied one so the entity still ends up with + // exactly one motion model. + for (const auto& common_plugin : entity_common_plugins[nm]) { + insert_entity_plugin(ent_desc_id, common_plugin); + } } } @@ -609,31 +664,60 @@ bool MissionParse::parse_mission() { // Loop through every other element under the "entity" node for (auto node = script_node.first_node(); node.is_valid(); node = node.next_sibling()) { std::string nm = node.name(); + std::string original_type = nm; // Save original type before modification + bool is_plugin = false; + int plugin_order = 0; + if (nm == "autonomy") { - nm += std::to_string(autonomy_order++); + is_plugin = true; + plugin_order = autonomy_order++; + nm += std::to_string(plugin_order); } else if (nm == "controller") { - nm += std::to_string(controller_order++); + is_plugin = true; + plugin_order = controller_order++; + nm += std::to_string(plugin_order); } else if (nm == "sensor") { - nm += std::to_string(sensor_order++); - } - - if (script_info.count(nm) > 0 && nm.compare("team_id")) { - LOG_WARN("Warning: entity contains multiple tags for \"" << nm << "\""); + is_plugin = true; + plugin_order = sensor_order++; + nm += std::to_string(plugin_order); + } else if (nm == "motion_model") { + is_plugin = true; + plugin_order = 0; // Only one motion model per entity } - script_info[nm] = trim(node.value()); + std::string node_value = trim(node.value()); - // Loop through each node's attributes: + // Collect inline XML attributes for this node. + // Plugin attrs go to entity_plugins_ for structured access. + std::map node_params; for (auto attr = node.first_attribute(); attr.is_valid(); attr = attr.next()) { const std::string attr_name = attr.name(); if (attr_name == "param_common") { for (auto& kv : param_common[attr.value()]) { - entity_attributes_[ent_desc_id][nm][kv.first] = kv.second; + node_params[kv.first] = kv.second; } } else { - entity_attributes_[ent_desc_id][nm][attr_name] = attr.value(); + node_params[attr_name] = attr.value(); } } + + if (is_plugin && !node_value.empty()) { + // Create EntityPluginInfo and store in entity_plugins_ + EntityPluginInfo plugin_info; + plugin_info.name = node_value; + plugin_info.type = original_type; + plugin_info.order = plugin_order; + plugin_info.params = node_params; + insert_entity_plugin(ent_desc_id, std::move(plugin_info)); + } + + if (!is_plugin) { + // Non-plugin nodes go into script_info as before + if (script_info.count(nm) > 0 && nm.compare("team_id")) { + LOG_WARN("Warning: entity contains multiple tags for \"" << nm << "\""); + } + script_info[nm] = node_value; + } } // For each entity, if the lat/lon are defined, use these values to @@ -757,8 +841,20 @@ bool MissionParse::parse_mission() { // If the entity block has a "tag" attribute, save the mapping from the // tag to the entity block ID auto tag_attr = script_node.first_attribute("tag"); + std::string entity_tag_str; if (tag_attr.is_valid()) { entity_tag_to_id_[tag_attr.value()] = ent_desc_id; + entity_tag_str = tag_attr.value(); + } + + // Populate entity_name and entity_tag for all plugins in this entity + std::string entity_name_str; + if (script_info.count("name") > 0) { + entity_name_str = script_info["name"]; + } + for (auto& kv : entity_plugins_[ent_desc_id]) { + kv.second.entity_name = entity_name_str; + kv.second.entity_tag = entity_tag_str; } entity_descs_[ent_desc_id++] = script_info; @@ -1028,10 +1124,6 @@ void MissionParse::set_log_dir(const std::string& log_dir) { log_dir_ = log_dir; } -std::map& MissionParse::entity_attributes() { - return entity_attributes_; -} - std::map>& MissionParse::entity_params() { return entity_params_; } @@ -1044,6 +1136,29 @@ EntityDesc_t& MissionParse::entity_descriptions() { return entity_descs_; } +const std::map>& MissionParse::all_entity_plugins() const { + return entity_plugins_; +} + +std::vector MissionParse::get_plugins_by_type( + int entity_block_id, const std::string& type) const { + std::vector result; + auto it = entity_plugins_.find(entity_block_id); + if (it != entity_plugins_.end()) { + for (const auto& kv : it->second) { + if (kv.second.type == type) { + result.push_back(kv.second); + } + } + // Sort by execution order + std::sort(result.begin(), result.end(), + [](const EntityPluginInfo& a, const EntityPluginInfo& b) { + return a.order < b.order; + }); + } + return result; +} + std::map& MissionParse::entity_tag_to_id() { return entity_tag_to_id_; } diff --git a/src/parse/MissionValidation.cpp b/src/parse/MissionValidation.cpp new file mode 100644 index 0000000000..6241298a77 --- /dev/null +++ b/src/parse/MissionValidation.cpp @@ -0,0 +1,278 @@ +/*! + * @file + * + * @section LICENSE + * + * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) + * + * This file is part of SCRIMMAGE. + * + * SCRIMMAGE is free software: you can redistribute it and/or modify it under + * the terms of the GNU Lesser General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public + * License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with SCRIMMAGE. If not, see . + * + * @author Ethan M Boos + * @date 06 April 2026 + * @version 0.1.0 + * @brief Brief file description. + * @section DESCRIPTION + * A Long description goes here. + * + */ + +#include "scrimmage/parse/MissionValidation.h" + +#include +#include +#include + +#include "scrimmage/common/FileSearch.h" +#include "scrimmage/parse/MissionParse.h" + +namespace fs = std::filesystem; + +namespace scrimmage { + +ValidationResult MissionValidation::validate( + const std::shared_ptr& mp, + FileSearch& file_search) { + + ValidationResult result; + + // Discover available plugins from SCRIMMAGE_PLUGIN_PATH, organized by type. + // Plugin type is determined from directory structure: plugins/// + // Works for both core SCRIMMAGE and external projects (they append to the path). + result.available_plugins = discover_plugins_by_type("SCRIMMAGE_PLUGIN_PATH", file_search); + + // Validate entity plugins: autonomy, controller, motion_model, sensor. + // These are attached to specific entities via blocks in the mission file. + for (const auto& [entity_desc_id, plugins] : mp->all_entity_plugins()) { + for (const auto& [key, plugin] : plugins) { + if (plugin.name.empty()) continue; + + const auto& available = result.available_plugins[plugin.type]; + bool found = available.count(plugin.name) > 0; + + PluginValidationInfo info; + info.status = found ? PluginValidationInfo::Status::valid + : PluginValidationInfo::Status::config_not_found; + + if (!found) { + info.suggestions = find_similar(plugin.name, available); + + ValidationError err; + err.type = ValidationError::Type::PLUGIN_CONFIG_NOT_FOUND; + err.plugin_name = plugin.name; + err.plugin_type = plugin.type; + err.context = "entity block " + std::to_string(entity_desc_id); + err.message = "Plugin '" + plugin.name + "' not found"; + err.suggestions = info.suggestions; + result.errors.push_back(err); + } + + result.entity_plugin_diagnostics[entity_desc_id][key] = info; + } + } + + // Validate global (simulation-level) plugins: entity_interaction, network, metrics. + // These are not attached to entities - they apply to the entire simulation. + auto validate_global = [&](const std::list& plugins, const std::string& type) { + for (const std::string& instance_name : plugins) { + auto attrs_it = mp->attributes().find(instance_name); + // Validation must check the implementation name, not the instance alias from name=. + const std::string plugin_name = + (attrs_it != mp->attributes().end()) + ? attrs_it->second["ORIGINAL_PLUGIN_NAME"] + : instance_name; + + const auto& available = result.available_plugins[type]; + bool found = available.count(plugin_name) > 0; + + if (!found) { + ValidationError err; + err.type = ValidationError::Type::PLUGIN_CONFIG_NOT_FOUND; + err.plugin_name = plugin_name; + err.plugin_type = type; + err.context = "global"; + err.message = type + " plugin '" + plugin_name + "' not found"; + err.suggestions = find_similar(plugin_name, available); + result.errors.push_back(err); + } + } + }; + + validate_global(mp->entity_interactions(), "entity_interaction"); + validate_global(mp->network_names(), "network"); + validate_global(mp->metrics(), "metrics"); + + return result; +} + +void MissionValidation::print_errors( + const ValidationResult& result, + const std::string& mission_filename) const { + + if (result.valid()) return; + + std::cerr << "\n"; + std::cerr << "========================================\n"; + std::cerr << "ERROR: Invalid mission file '" << mission_filename << "'\n"; + std::cerr << "========================================\n\n"; + + for (const auto& err : result.errors) { + std::cerr << " [" << err.plugin_type << "] Plugin config not found\n"; + std::cerr << " Plugin: '" << err.plugin_name << "'\n"; + + if (!err.context.empty() && err.context != "global") { + std::cerr << " In: " << err.context << "\n"; + } + + if (!err.suggestions.empty()) { + std::cerr << " Did you mean: "; + for (size_t i = 0; i < err.suggestions.size(); ++i) { + if (i > 0) std::cerr << ", "; + std::cerr << "'" << err.suggestions[i] << "'"; + } + std::cerr << " ?\n"; + } + std::cerr << "\n"; + } + + std::cerr << "Hint: Plugins are searched in SCRIMMAGE_PLUGIN_PATH.\n"; + const char* path = std::getenv("SCRIMMAGE_PLUGIN_PATH"); + if (path) { + std::cerr << " Current SCRIMMAGE_PLUGIN_PATH: " << path << "\n"; + } else { + std::cerr << " Warning: SCRIMMAGE_PLUGIN_PATH is not set!\n"; + } + std::cerr << "\n"; +} + +// Discover plugins organized by type from directory structure. +// Path format: ...//plugins///.xml +// Maps directory names to our type names (e.g., "motion" -> "motion_model") +// +// IMPORTANT: Path Structure Requirement for Submodules +// ----------------------------------------------------- +// This function determines plugin type by parsing the directory structure, +// looking for a "plugins" directory followed by a type directory +// (autonomy, controller, motion, sensor, interaction, metrics, network). +// +// The "plugins" directory can be nested at any depth — we search for it anywhere +// in the path. These all work: +// +// submodule/plugins/autonomy/MyAutonomy/MyAutonomy.xml <-- WORKS +// submodule/src/plugins/autonomy/MyAutonomy/MyAutonomy.xml <-- WORKS +// external/lib/plugins/motion/MyMotion/MyMotion.xml <-- WORKS +// +// What matters is: plugins///.xml +// +// This will NOT work: +// submodule/MyAutonomy/MyAutonomy.xml <-- NO "plugins" dir +// submodule/autonomy/MyAutonomy/MyAutonomy.xml <-- NO "plugins" dir +// +// NOTE: Plugin XML config files do NOT contain type information — they only have +// PluginName_plugin and plugin-specific params. The directory +// structure is the ONLY source of truth for plugin type. +std::map> MissionValidation::discover_plugins_by_type( + const std::string& env_var, + FileSearch& file_search) { + + std::map> plugins_by_type; + std::unordered_map> xml_files; + file_search.find_files(env_var, ".xml", xml_files, verbose_); + + for (const auto& kv : xml_files) { + const std::string& filename = kv.first; + const std::list& paths = kv.second; + + if (filename.size() <= 4 || filename.substr(filename.size() - 4) != ".xml") { + continue; + } + std::string plugin_name = filename.substr(0, filename.size() - 4); + + // Extract type from path: .../plugins///... + if (paths.empty()) continue; + fs::path p(paths.front()); + auto it = std::find(p.begin(), p.end(), "plugins"); + if (it != p.end() && ++it != p.end()) { + auto type_it = kPluginDirToType.find(it->string()); + if (type_it != kPluginDirToType.end()) { + plugins_by_type[type_it->second].insert(plugin_name); + } + } + } + return plugins_by_type; +} + +// Find plugin names similar to the typo using Levenshtein (edit) distance. +// Edit distance = minimum insertions, deletions, or substitutions to transform +// one string into another. Low distance means likely typo. +// e.g., "Straigt" -> "Straight" has distance 1 (insert 'h') +std::vector MissionValidation::find_similar( + const std::string& name, + const std::set& available, + size_t max_suggestions) const { + + std::vector> scored; + + std::string name_lower = name; + std::transform(name_lower.begin(), name_lower.end(), name_lower.begin(), ::tolower); + + for (const auto& candidate : available) { + std::string cand_lower = candidate; + std::transform(cand_lower.begin(), cand_lower.end(), cand_lower.begin(), ::tolower); + + size_t dist = levenshtein_distance(name_lower, cand_lower); + if (dist <= (name.size() / 2 + 2)) { + scored.push_back({dist, candidate}); + } + } + + std::sort(scored.begin(), scored.end()); + + std::vector suggestions; + for (size_t i = 0; i < std::min(max_suggestions, scored.size()); ++i) { + suggestions.push_back(scored[i].second); + } + return suggestions; +} + +size_t MissionValidation::levenshtein_distance( + const std::string& s1, + const std::string& s2) const { + + size_t m = s1.size(); + size_t n = s2.size(); + + if (m == 0) return n; + if (n == 0) return m; + + std::vector> dp(m + 1, std::vector(n + 1)); + + for (size_t i = 0; i <= m; ++i) dp[i][0] = i; + for (size_t j = 0; j <= n; ++j) dp[0][j] = j; + + for (size_t i = 1; i <= m; ++i) { + for (size_t j = 1; j <= n; ++j) { + size_t cost = (s1[i - 1] == s2[j - 1]) ? 0 : 1; + dp[i][j] = std::min({ + dp[i - 1][j] + 1, + dp[i][j - 1] + 1, + dp[i - 1][j - 1] + cost + }); + } + } + return dp[m][n]; +} + +} // namespace scrimmage diff --git a/src/plugins/autonomy/CMakeLists.txt b/src/plugins/autonomy/CMakeLists.txt index 968c8a9ad2..1008ae71bc 100644 --- a/src/plugins/autonomy/CMakeLists.txt +++ b/src/plugins/autonomy/CMakeLists.txt @@ -1,4 +1,5 @@ add_subdirectory(APITester) +add_subdirectory(RuntimeOverrideSpawner) add_subdirectory(ArduPilot) add_subdirectory(AuctionAssign) add_subdirectory(AutonomyExecutor) diff --git a/src/plugins/metrics/OpenAIRewards/CMakeLists.txt b/src/plugins/autonomy/RuntimeOverrideSpawner/CMakeLists.txt similarity index 83% rename from src/plugins/metrics/OpenAIRewards/CMakeLists.txt rename to src/plugins/autonomy/RuntimeOverrideSpawner/CMakeLists.txt index 307dce4c04..4ecd305e79 100644 --- a/src/plugins/metrics/OpenAIRewards/CMakeLists.txt +++ b/src/plugins/autonomy/RuntimeOverrideSpawner/CMakeLists.txt @@ -1,20 +1,21 @@ #-------------------------------------------------------- # Library Creation #-------------------------------------------------------- -set(LIBRARY_NAME OpenAIRewards_plugin) +set(LIBRARY_NAME RuntimeOverrideSpawner_plugin) set(LIB_MAJOR 0) set(LIB_MINOR 0) set(LIB_RELEASE 1) -SET(SRCS OpenAIRewards.cpp ) - +set(SRCS RuntimeOverrideSpawner.cpp) add_library(${LIBRARY_NAME} SHARED ${SRCS} ) target_compile_options(${LIBRARY_NAME} - PRIVATE -Wall + PRIVATE + -Wall + $<$:"-Wno-return-type-c-linkage"> ) target_link_libraries(${LIBRARY_NAME} @@ -36,11 +37,9 @@ target_include_directories(${LIBRARY_NAME} ) install(TARGETS ${LIBRARY_NAME} - # IMPORTANT: Add the library to the "export-set" EXPORT ${PROJECT_NAME}-targets RUNTIME DESTINATION bin LIBRARY DESTINATION lib/${PROJECT_NAME}/plugin_libs ) -# Push up the PROJECT_PLUGINS variable -set(PROJECT_PLUGINS ${PROJECT_PLUGINS} ${LIBRARY_NAME} PARENT_SCOPE) +set(PROJECT_PLUGINS ${PROJECT_PLUGINS} ${LIBRARY_NAME} PARENT_SCOPE) \ No newline at end of file diff --git a/src/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.cpp b/src/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.cpp new file mode 100644 index 0000000000..90d5eafa7e --- /dev/null +++ b/src/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.cpp @@ -0,0 +1,146 @@ +#include "scrimmage/plugins/autonomy/RuntimeOverrideSpawner/RuntimeOverrideSpawner.h" + +#include +#include +#include +#include + +#include "scrimmage/log/Logger.h" +#include "scrimmage/math/State.h" +#include "scrimmage/msgs/Event.pb.h" +#include "scrimmage/parse/ParseUtils.h" +#include "scrimmage/plugin_manager/RegisterPlugin.h" +#include "scrimmage/proto/ProtoConversions.h" +#include "scrimmage/pubsub/Message.h" +#include "scrimmage/pubsub/Publisher.h" + +namespace sc = scrimmage; +namespace sm = scrimmage_msgs; + +REGISTER_PLUGIN( + scrimmage::Autonomy, + scrimmage::autonomy::RuntimeOverrideSpawner, + RuntimeOverrideSpawner_plugin) + +namespace scrimmage { +namespace autonomy { + +namespace { + +std::string zero_pad_index(int index, int width) { + std::ostringstream stream; + stream << std::setw(width) << std::setfill('0') << index; + return stream.str(); +} + +} // namespace + +void RuntimeOverrideSpawner::init(std::map& params) { + // This plugin demonstrates how to publish GenerateEntity messages with + // runtime plugin parameter overrides. Copy and adapt this pattern for + // custom spawning logic. + pub_gen_ents_ = advertise("GlobalNetwork", "GenerateEntity"); + + // Layout/spawning configuration - kept configurable via XML + entity_tag_ = sc::get("entity_tag", params, entity_tag_); + name_prefix_ = sc::get("name_prefix", params, name_prefix_); + spawn_count_ = std::max(0, sc::get("spawn_count", params, spawn_count_)); + columns_ = std::max(1, sc::get("columns", params, columns_)); + x_spacing_ = sc::get("x_spacing", params, x_spacing_); + y_spacing_ = sc::get("y_spacing", params, y_spacing_); + z_spacing_ = sc::get("z_spacing", params, z_spacing_); + x_offset_ = sc::get("x_offset", params, x_offset_); + y_offset_ = sc::get("y_offset", params, y_offset_); + z_offset_ = sc::get("z_offset", params, z_offset_); + spawn_time_ = sc::get("spawn_time", params, spawn_time_); + + desired_altitude_idx_ = + vars_.declare(VariableIO::Type::desired_altitude, VariableIO::Direction::Out); + desired_heading_idx_ = + vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); + desired_speed_idx_ = + vars_.declare(VariableIO::Type::desired_speed, VariableIO::Direction::Out); +} + +bool RuntimeOverrideSpawner::step_autonomy(double t, double dt) { + static_cast(dt); + + // Keep the host entity valid for UnicyclePID even though it is only acting + // as a publisher for runtime-generated children. + vars_.output(desired_altitude_idx_, state_->pos()(2)); + vars_.output(desired_heading_idx_, state_->quat().yaw()); + vars_.output(desired_speed_idx_, 0.0); + + if (spawned_ || t < spawn_time_ || spawn_count_ <= 0) { + return true; + } + + // Publish the whole batch once. The selected entity_tag points at a + // zero-count template in the mission file, and each message applies + // runtime plugin parameter overrides to that template before spawn. + spawned_ = true; + const Eigen::Vector3d anchor = state_->pos(); + + for (int index = 0; index < spawn_count_; ++index) { + const int row = index / columns_; + const int col = index % columns_; + publish_spawn(index, row, col, anchor); + } + + LOG_INFO("RuntimeOverrideSpawner: published " << spawn_count_ + << " GenerateEntity messages for tag '" << entity_tag_ << "'."); + return true; +} + +void RuntimeOverrideSpawner::publish_spawn( + int index, + int row, + int col, + const Eigen::Vector3d& anchor) { + const int index_width = + std::max(1, std::to_string(std::max(0, spawn_count_ - 1)).size()); + const std::string formatted_index = zero_pad_index(index, index_width); + + State spawned_state(*state_); + spawned_state.pos() = anchor + Eigen::Vector3d( + x_offset_ + col * x_spacing_, + y_offset_ + row * y_spacing_, + z_offset_ + row * z_spacing_); + + // --- Example: Build a GenerateEntity message with plugin overrides --- + auto msg = std::make_shared>(); + + // Set the entity template to spawn from (must exist in mission XML with count=0) + msg->data.set_entity_tag(entity_tag_); + + // Set the spawn position/orientation + sc::set(msg->data.mutable_state(), spawned_state); + + // Optional: override entity-level params (name, color, etc.) + if (!name_prefix_.empty()) { + auto* name_param = msg->data.add_entity_param(); + name_param->set_key("name"); + name_param->set_value(name_prefix_ + "_" + formatted_index); + } + + // --- Plugin parameter overrides --- + // Target a plugin by type and name (preferred) or type and index + auto* plugin_override = msg->data.add_plugin_override(); + plugin_override->set_plugin_type("autonomy"); + plugin_override->set_plugin_name("APITester"); // Target by name, not index + + // Add parameter overrides for the targeted plugin + auto* int_param = plugin_override->add_params(); + int_param->set_key("my_test_int"); + int_param->set_value(std::to_string(1000 + index)); + + auto* csv_param = plugin_override->add_params(); + csv_param->set_key("csv_file_name"); + csv_param->set_value("runtime_override_stress_" + formatted_index + ".csv"); + + // Publish the message — SimControl will spawn the entity with overrides applied + pub_gen_ents_->publish(msg); +} + +} // namespace autonomy +} // namespace scrimmage \ No newline at end of file diff --git a/src/plugins/metrics/CMakeLists.txt b/src/plugins/metrics/CMakeLists.txt index 4c06889583..ae0169bfff 100644 --- a/src/plugins/metrics/CMakeLists.txt +++ b/src/plugins/metrics/CMakeLists.txt @@ -1,6 +1,5 @@ add_subdirectory(CPA) add_subdirectory(FlagCaptureMetrics) -add_subdirectory(OpenAIRewards) add_subdirectory(SimpleCaptureMetrics) add_subdirectory(SimpleCollisionMetrics) diff --git a/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp b/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp deleted file mode 100644 index 53679375cc..0000000000 --- a/src/plugins/metrics/OpenAIRewards/OpenAIRewards.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/*! - * @file - * - * @section LICENSE - * - * Copyright (C) 2017 by the Georgia Tech Research Institute (GTRI) - * - * This file is part of SCRIMMAGE. - * - * SCRIMMAGE is free software: you can redistribute it and/or modify it under - * the terms of the GNU Lesser General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * SCRIMMAGE is distributed in the hope that it will be useful, but WITHOUT - * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or - * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public - * License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with SCRIMMAGE. If not, see . - * - * @author Kevin DeMarco - * @author Eric Squires - * @date 31 July 2017 - * @version 0.1.0 - * @brief Brief file description. - * @section DESCRIPTION - * A Long description goes here. - * - */ - -#include "scrimmage/plugins/metrics/OpenAIRewards/OpenAIRewards.h" - -#include -#include -#include - -#include "scrimmage/common/CSV.h" -#include "scrimmage/common/Utilities.h" -#include "scrimmage/entity/Entity.h" -#include "scrimmage/log/Logger.h" -#include "scrimmage/math/State.h" -#include "scrimmage/metrics/Metrics.h" -#include "scrimmage/msgs/Collision.pb.h" -#include "scrimmage/msgs/Event.pb.h" -#include "scrimmage/parse/MissionParse.h" -#include "scrimmage/parse/ParseUtils.h" -#include "scrimmage/plugin_manager/RegisterPlugin.h" -#include "scrimmage/pubsub/Message.h" -#include "scrimmage/pubsub/Subscriber.h" - -namespace sc = scrimmage; -namespace sm = scrimmage_msgs; - -REGISTER_PLUGIN(scrimmage::Metrics, scrimmage::metrics::OpenAIRewards, OpenAIRewards_plugin) - -namespace scrimmage { -namespace metrics { - -OpenAIRewards::OpenAIRewards() : Metrics() { - print_team_summary_ = false; -} - -void OpenAIRewards::init(std::map& /*params*/) { - auto cb = [&](auto msg) { - size_t id; - double reward; - std::tie(id, reward) = msg->data; - auto it = rewards_.find(id); - if (it == rewards_.end()) { - rewards_[id] = reward; - } else { - it->second += reward; - } - print_team_summary_ = true; - }; - subscribe>("GlobalNetwork", "reward", cb); -} - -void OpenAIRewards::print_team_summaries() { - for (auto& kv : rewards_) { - std::stringstream ss; - ss << "Reward for id " << kv.first << " = " << kv.second; - LOG_INFO(ss.str()); - } -} - -void OpenAIRewards::calc_team_scores() { - CSV csv; - std::string filename = parent_->mp()->log_dir() + "/rewards.csv"; - - if (!csv.open_output(filename)) { - LOG_ERROR("Couldn't create output file"); - } - - csv.set_column_headers("id, reward"); - for (auto& kv : rewards_) { - csv.append(CSV::Pairs{{"id", kv.first}, {"reward", kv.second}}); - } - csv.close_output(); -} - -} // namespace metrics -} // namespace scrimmage diff --git a/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp b/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp index 3275cf31aa..e5a61c2158 100644 --- a/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp +++ b/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp @@ -396,7 +396,7 @@ bool FixedWing6DOF::step(double time, double dt) { // draw velocity if (draw_vel_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 255, 0, 0); sc::set(line->mutable_line()->mutable_start(), state_->pos()); @@ -406,7 +406,7 @@ bool FixedWing6DOF::step(double time, double dt) { // draw angular velocity if (draw_ang_vel_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 0, 255, 0); sc::set(line->mutable_line()->mutable_start(), state_->pos()); diff --git a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp index 6fbbbf8d5c..cb24fcb1bf 100644 --- a/src/plugins/motion/JSBSimControl/JSBSimControl.cpp +++ b/src/plugins/motion/JSBSimControl/JSBSimControl.cpp @@ -322,7 +322,7 @@ bool JSBSimControl::step(double time, double dt) { // draw velocity if (draw_vel_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 255, 0, 0); sc::set(line->mutable_line()->mutable_start(), state_->pos()); @@ -332,7 +332,7 @@ bool JSBSimControl::step(double time, double dt) { // draw angular velocity if (draw_ang_vel_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 0, 255, 0); sc::set(line->mutable_line()->mutable_start(), state_->pos()); @@ -342,7 +342,7 @@ bool JSBSimControl::step(double time, double dt) { // draw acceleration if (draw_acc_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 0, 0, 255); sc::set(line->mutable_line()->mutable_start(), state_->pos()); diff --git a/src/plugins/motion/Multirotor/Multirotor.cpp b/src/plugins/motion/Multirotor/Multirotor.cpp index e150b5486d..a10f05afaa 100644 --- a/src/plugins/motion/Multirotor/Multirotor.cpp +++ b/src/plugins/motion/Multirotor/Multirotor.cpp @@ -230,7 +230,7 @@ bool Multirotor::step(double time, double dt) { // draw velocity if (show_shapes_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 255, 255, 0); sc::set(line->mutable_line()->mutable_start(), state_->pos()); @@ -240,7 +240,7 @@ bool Multirotor::step(double time, double dt) { // draw angular velocity if (show_shapes_) { - sc::ShapePtr line(new sp::Shape()); + sc::ShapePtr line(new scrimmage_proto::Shape()); line->set_opacity(1.0); sc::set(line->mutable_color(), 0, 255, 255); sc::set(line->mutable_line()->mutable_start(), state_->pos()); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 81518e39aa..e358599e50 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -43,6 +43,7 @@ #include "scrimmage/common/Utilities.h" #include "scrimmage/entity/Contact.h" #include "scrimmage/entity/Entity.h" +#include "scrimmage/entity/RuntimePluginOverrides.h" #include "scrimmage/log/Log.h" #include "scrimmage/log/Logger.h" #include "scrimmage/metrics/Metrics.h" @@ -51,6 +52,7 @@ #include "scrimmage/network/Interface.h" #include "scrimmage/parse/ConfigParse.h" #include "scrimmage/parse/MissionParse.h" +#include "scrimmage/parse/MissionValidation.h" #include "scrimmage/parse/ParseUtils.h" #include "scrimmage/plugin_manager/PluginManager.h" #include "scrimmage/sensor/Sensor.h" @@ -201,6 +203,17 @@ bool SimControl::init(const std::string& mission_file, const bool& init_python) } setup_logging(); + // Validate all plugins referenced in the mission file + // This catches invalid plugin names before simulation starts + { + MissionValidation validator; + ValidationResult validation_result = validator.validate(mp_, *file_search_); + if (!validation_result.valid()) { + validator.print_errors(validation_result, mission_file); + return false; + } + } + #if ENABLE_GPU_ACCELERATION == 1 // Needs to be done after parsing mission file init_gpu(); @@ -333,19 +346,28 @@ bool SimControl::generate_entity(const int& ent_desc_id) { return false; } - // Get the entity attributes for the given id - AttributeMap plugin_attr_map = mp_->entity_attributes()[ent_desc_id]; + return generate_entity(ent_desc_id, it_params->second); +} - return generate_entity(ent_desc_id, it_params->second, plugin_attr_map); +bool SimControl::generate_entity( + const int& ent_desc_id, + std::map& params) { + const RuntimePluginOverrides runtime_plugin_overrides; + return generate_entity(ent_desc_id, params, runtime_plugin_overrides); } bool SimControl::generate_entity( const int& ent_desc_id, std::map& params, - AttributeMap& plugin_attr_map) { + const RuntimePluginOverrides& runtime_plugin_overrides) { #if ENABLE_JSBSIM == 1 params["JSBSIM_ROOT"] = jsbsim_root_; #endif + + if (!validate_runtime_plugin_overrides(mp_, file_search_, ent_desc_id, runtime_plugin_overrides)) { + return false; + } + params["dt"] = std::to_string(dt_); params["motion_multiplier"] = std::to_string(mp_->motion_multiplier()); @@ -428,8 +450,8 @@ bool SimControl::generate_entity( info.gpu = gpu_; EntityInitParams init_params; - init_params.overrides = plugin_attr_map; init_params.info = params; + init_params.runtime_plugin_overrides = runtime_plugin_overrides; init_params.id = id; init_params.ent_desc_id = ent_desc_id; init_params.param_override_func = [](std::map&) {}; @@ -949,6 +971,7 @@ bool SimControl::start() { // Set subscriber / callback that allows plugins to generate entities auto gen_ent_cb = [&](auto& msg) { + RuntimePluginOverrides runtime_plugin_overrides; auto it_ent_desc_id = mp_->entity_tag_to_id().find(msg->data.entity_tag()); if (it_ent_desc_id == mp_->entity_tag_to_id().end()) { LOG_ERROR("Failed to find entity_tag, " << msg->data.entity_tag() @@ -981,22 +1004,30 @@ bool SimControl::start() { // Assign the ID based on the protobuf message params["id"] = std::to_string(msg->data.entity_id()); - // Override any manually specified entity_params - for (int i = 0; i < msg->data.entity_param().size(); i++) { - params[msg->data.entity_param(i).key()] = msg->data.entity_param(i).value(); + const int ent_desc_id = it_ent_desc_id->second; + if (!parse_runtime_plugin_overrides(msg->data, mp_, ent_desc_id, runtime_plugin_overrides)) { + return; } - AttributeMap plugin_attr_map = mp_->entity_attributes()[it_ent_desc_id->second]; - for (int i = 0; i < msg->data.plugin_param().size(); i++) { - plugin_attr_map[msg->data.plugin_param(i).plugin_type()] - [msg->data.plugin_param(i).tag_name()] = - msg->data.plugin_param(i).tag_value(); + // Override any manually specified entity_params + // NOTE: Plugin names (autonomy, controller, motion_model, sensor) cannot be + // overridden at runtime. These are set in the mission XML and cannot be changed at spawn time. + for (int i = 0; i < msg->data.entity_param().size(); i++) { + const std::string& key = msg->data.entity_param(i).key(); + if (is_plugin_name_override_key(key)) { + LOG_WARN("GenerateEntity: Ignoring plugin override '" << key << "=" + << msg->data.entity_param(i).value() << "'. " + << "Plugin names cannot be changed at runtime. " + << "Use separate entity templates with different tags instead."); + continue; + } + params[key] = msg->data.entity_param(i).value(); } // Recreate the rtree with one additional size for this entity. this->create_rtree(1); - if (not this->generate_entity(it_ent_desc_id->second, params, plugin_attr_map)) { + if (not this->generate_entity(it_ent_desc_id->second, params, runtime_plugin_overrides)) { LOG_ERROR("Failed to generate entity with tag: " << msg->data.entity_tag()); return; } diff --git a/src/simcontrol/SimUtils.cpp b/src/simcontrol/SimUtils.cpp index fda30805af..b0c1710ce7 100644 --- a/src/simcontrol/SimUtils.cpp +++ b/src/simcontrol/SimUtils.cpp @@ -66,7 +66,6 @@ bool create_ent_inters( for (std::string ent_inter_name : info.mp->entity_interactions()) { ConfigParse config_parse; std::map& overrides = info.mp->attributes()[ent_inter_name]; - PluginStatus status = info.plugin_manager->make_plugin( "scrimmage::EntityInteraction", @@ -130,7 +129,6 @@ bool create_metrics( for (std::string metrics_name : info.mp->metrics()) { ConfigParse config_parse; std::map& overrides = info.mp->attributes()[metrics_name]; - PluginStatus status = info.plugin_manager->make_plugin( "scrimmage::Metrics", metrics_name, @@ -242,7 +240,6 @@ bool create_networks( for (std::string network_name : info.mp->network_names()) { ConfigParse config_parse; std::map& overrides = info.mp->attributes()[network_name]; - PluginStatus status = info.plugin_manager->make_plugin( "scrimmage::Network", network_name, diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 3cbd59a7cd..142a8ca8b9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -9,6 +9,7 @@ set(test_files test_find_mission.cpp test_id.cpp test_mission_parse.cpp + test_mission_validation.cpp test_params.cpp test_parse_utils.cpp test_quaternion.cpp diff --git a/test/test_entity_configs.cpp b/test/test_entity_configs.cpp index 70c91b01c1..74c80f0053 100644 --- a/test/test_entity_configs.cpp +++ b/test/test_entity_configs.cpp @@ -33,10 +33,52 @@ #include #include +#include +#include +#include +#include +#include + +#include "scrimmage/parse/MissionParse.h" +#include "scrimmage/simcontrol/SimControl.h" #include "scrimmage/simcontrol/SimUtils.h" namespace sc = scrimmage; +namespace { + +std::string read_file_contents(const std::filesystem::path& file_path) { + std::ifstream file(file_path); + std::stringstream buffer; + buffer << file.rdbuf(); + return buffer.str(); +} + +std::vector find_prefixed_csvs( + const std::filesystem::path& dir, + const std::string& prefix) { + std::vector matches; + for (const auto& entry : std::filesystem::directory_iterator(dir)) { + if (!entry.is_regular_file()) { + continue; + } + const auto& path = entry.path(); + if (path.extension() != ".csv") { + continue; + } + + const std::string filename = path.filename().string(); + if (filename.rfind(prefix, 0) == 0) { + matches.push_back(path); + } + } + + std::sort(matches.begin(), matches.end()); + return matches; +} + +} // namespace + TEST(test_entity_configs, valid_entity_configs) { const std::string mission = "test_valid_entity_configs"; auto log_dir = sc::run_test(mission, false, false); @@ -61,6 +103,97 @@ TEST(test_entity_configs, missing_controller) { EXPECT_FALSE(success); } +TEST(test_entity_configs, generate_entity_runtime_plugin_overrides) { + sc::SimControl simcontrol; + ASSERT_TRUE(simcontrol.init("missions/test/test_generate_entity_runtime_overrides.xml", false)); + ASSERT_TRUE(simcontrol.start()); + + auto& params = simcontrol.mp()->entity_descriptions().at(0); + sc::RuntimePluginOverrides plugin_overrides; + plugin_overrides["autonomy"][0]["my_test_int"] = "42"; + plugin_overrides["autonomy"][0]["csv_file_name"] = "runtime_override.csv"; + + EXPECT_TRUE(simcontrol.generate_entity(0, params, plugin_overrides)); + + const std::filesystem::path csv_path = + std::filesystem::path(simcontrol.mp()->log_dir()) / "runtime_override.csv"; + ASSERT_TRUE(std::filesystem::exists(csv_path)); + + const std::string csv_contents = read_file_contents(csv_path); + EXPECT_NE(csv_contents.find("my_test_bool,my_test_int,my_test_float,my_test_double"), std::string::npos); + EXPECT_NE(csv_contents.find("0,42,1,100"), std::string::npos); + + EXPECT_TRUE(simcontrol.shutdown(false)); +} + +TEST(test_entity_configs, generate_entity_runtime_plugin_override_rejects_missing_plugin_index) { + sc::SimControl simcontrol; + ASSERT_TRUE(simcontrol.init("missions/test/test_generate_entity_runtime_overrides.xml", false)); + ASSERT_TRUE(simcontrol.start()); + + auto& params = simcontrol.mp()->entity_descriptions().at(0); + sc::RuntimePluginOverrides plugin_overrides; + plugin_overrides["autonomy"][1]["my_test_int"] = "42"; + + EXPECT_FALSE(simcontrol.generate_entity(0, params, plugin_overrides)); + + EXPECT_TRUE(simcontrol.shutdown(false)); +} + +TEST(test_entity_configs, generate_entity_runtime_plugin_override_rejects_unknown_param_name) { + sc::SimControl simcontrol; + ASSERT_TRUE(simcontrol.init("missions/test/test_generate_entity_runtime_overrides.xml", false)); + ASSERT_TRUE(simcontrol.start()); + + auto& params = simcontrol.mp()->entity_descriptions().at(0); + sc::RuntimePluginOverrides plugin_overrides; + plugin_overrides["autonomy"][0]["definitely_not_a_real_param"] = "42"; + + EXPECT_FALSE(simcontrol.generate_entity(0, params, plugin_overrides)); + + EXPECT_TRUE(simcontrol.shutdown(false)); +} + +TEST(test_entity_configs, generate_entity_runtime_plugin_override_allows_unknown_param_name_when_disabled) { + sc::SimControl simcontrol; + ASSERT_TRUE(simcontrol.init("missions/test/test_generate_entity_runtime_overrides.xml", false)); + ASSERT_TRUE(simcontrol.start()); + simcontrol.mp()->params()["strict_runtime_plugin_params"] = "false"; + + auto& params = simcontrol.mp()->entity_descriptions().at(0); + sc::RuntimePluginOverrides plugin_overrides; + plugin_overrides["autonomy"][0]["definitely_not_a_real_param"] = "42"; + plugin_overrides["autonomy"][0]["csv_file_name"] = "runtime_override_unknown_allowed.csv"; + + EXPECT_TRUE(simcontrol.generate_entity(0, params, plugin_overrides)); + + const std::filesystem::path csv_path = + std::filesystem::path(simcontrol.mp()->log_dir()) / "runtime_override_unknown_allowed.csv"; + ASSERT_TRUE(std::filesystem::exists(csv_path)); + + EXPECT_TRUE(simcontrol.shutdown(false)); +} + +TEST(test_entity_configs, generate_entity_runtime_plugin_overrides_stress_mission) { + auto log_dir = sc::run_test("missions/test/test_generate_entity_runtime_override_stress.xml", false, false); + ASSERT_TRUE(log_dir); + + const std::filesystem::path log_dir_path(*log_dir); + const auto csv_files = find_prefixed_csvs(log_dir_path, "runtime_override_stress_"); + ASSERT_EQ(csv_files.size(), 120); + + const std::filesystem::path first_csv = log_dir_path / "runtime_override_stress_000.csv"; + const std::filesystem::path last_csv = log_dir_path / "runtime_override_stress_119.csv"; + ASSERT_TRUE(std::filesystem::exists(first_csv)); + ASSERT_TRUE(std::filesystem::exists(last_csv)); + + const std::string first_csv_contents = read_file_contents(first_csv); + const std::string last_csv_contents = read_file_contents(last_csv); + + EXPECT_NE(first_csv_contents.find("0,1000,1,100"), std::string::npos); + EXPECT_NE(last_csv_contents.find("0,1119,1,100"), std::string::npos); +} + int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/test/test_mission_parse.cpp b/test/test_mission_parse.cpp index ec59bffff7..ab7d164391 100644 --- a/test/test_mission_parse.cpp +++ b/test/test_mission_parse.cpp @@ -33,6 +33,22 @@ #include "scrimmage/parse/MissionParse.h" +namespace { + +const std::map& get_entity_plugins( + const scrimmage::MissionParse& mission_parse, int entity_block_id) { + static const std::map empty_plugins; + const auto& all_plugins = mission_parse.all_entity_plugins(); + auto plugin_it = all_plugins.find(entity_block_id); + EXPECT_NE(plugin_it, all_plugins.end()); + if (plugin_it == all_plugins.end()) { + return empty_plugins; + } + return plugin_it->second; +} + +} // namespace + class MissionParseTest : public testing::TestWithParam { protected: void SetUp() override { @@ -117,8 +133,16 @@ TEST_P(MissionParseTest, mission_parse_test_unique_tags) { TEST_P(MissionParseTest, mission_parse_test_param_common) { compare_attributes(mp.attributes(), "LocalNetwork", {"csv_filename"}, {"bar.csv"}); - // Autonomies are stored as "autonomy0", "autonomy1", etc... - compare_attributes(mp.entity_attributes()[0], "autonomy0", {"speed"}, {"20"}); + auto &network_names = mp.network_names(); + EXPECT_NE(std::find(network_names.begin(), network_names.end(), "CommsNetwork"), + network_names.end()); + EXPECT_EQ(mp.attributes()["CommsNetwork"]["ORIGINAL_PLUGIN_NAME"], "SphereNetwork"); + + // Plugin params are now in all_entity_plugins() + const auto& plugins = get_entity_plugins(mp, 0); + auto it = plugins.find("autonomy:Straight"); + ASSERT_NE(it, plugins.end()); + EXPECT_EQ(it->second.params.at("speed"), "20"); } TEST_P(MissionParseTest, mission_parse_test_gen_info) { @@ -184,6 +208,7 @@ TEST_P(MissionParseTest, mission_parse_test_entities) { }; auto descriptor_it = descriptions.begin(); + // Entity properties (plugins are now in all_entity_plugins()) std::map expected_map{ std::make_pair("name", "uav_entity"), std::make_pair("team_id", "1"), @@ -198,12 +223,30 @@ TEST_P(MissionParseTest, mission_parse_test_entities) { std::make_pair("z", "195"), std::make_pair("heading", "0"), std::make_pair("visual_model", "zephyr-blue"), - std::make_pair("autonomy0", "Straight"), - std::make_pair("controller0", "SimpleAircraftControllerPID"), - std::make_pair("motion_model", "SimpleAircraft"), }; compare_entity_descriptor(descriptor_it->second, expected_map); + + // Verify plugins are in all_entity_plugins() + const auto& plugins = get_entity_plugins(mp, 0); + ASSERT_EQ(plugins.size(), 3); // autonomy, controller, motion_model + + auto it_autonomy = plugins.find("autonomy:Straight"); + ASSERT_NE(it_autonomy, plugins.end()); + EXPECT_EQ(it_autonomy->second.name, "Straight"); + EXPECT_EQ(it_autonomy->second.type, "autonomy"); + EXPECT_EQ(it_autonomy->second.order, 0); + EXPECT_EQ(it_autonomy->second.entity_name, "uav_entity"); + + auto it_controller = plugins.find("controller:SimpleAircraftControllerPID"); + ASSERT_NE(it_controller, plugins.end()); + EXPECT_EQ(it_controller->second.name, "SimpleAircraftControllerPID"); + EXPECT_EQ(it_controller->second.type, "controller"); + + auto it_motion = plugins.find("motion_model:SimpleAircraft"); + ASSERT_NE(it_motion, plugins.end()); + EXPECT_EQ(it_motion->second.name, "SimpleAircraft"); + EXPECT_EQ(it_motion->second.type, "motion_model"); } TEST_P(MissionParseTest, mission_parse_test_team_info) { @@ -242,6 +285,62 @@ TEST_P(MissionParseTest, mission_parse_test_team_info) { compare_team_info(team_it->second, expected_team_info); } +TEST(MissionParseStandaloneTest, entity_common_plugins_are_imported_into_entity_plugins) { + scrimmage::MissionParse mp; + ASSERT_TRUE(mp.parse("missions/test/test_entity_common_plugins.xml")); + + const auto& plugins = get_entity_plugins(mp, 0); + ASSERT_EQ(plugins.size(), 5); + + auto it_common_auto = plugins.find("autonomy:Straight"); + ASSERT_NE(it_common_auto, plugins.end()); + EXPECT_EQ(it_common_auto->second.order, 0); + EXPECT_EQ(it_common_auto->second.params.at("speed"), "10"); + + auto it_entity_auto = plugins.find("autonomy:Straight:1"); + ASSERT_NE(it_entity_auto, plugins.end()); + EXPECT_EQ(it_entity_auto->second.order, 1); + EXPECT_EQ(it_entity_auto->second.params.at("speed"), "20"); + + auto it_controller = plugins.find("controller:SimpleAircraftControllerPID"); + ASSERT_NE(it_controller, plugins.end()); + EXPECT_EQ(it_controller->second.order, 0); + EXPECT_EQ(it_controller->second.params.at("gain"), "1.5"); + + auto it_motion = plugins.find("motion_model:SimpleAircraft"); + ASSERT_NE(it_motion, plugins.end()); + EXPECT_EQ(it_motion->second.order, 0); + EXPECT_EQ(it_motion->second.params.at("max_speed"), "25"); + + auto it_sensor = plugins.find("sensor:NoisyState"); + ASSERT_NE(it_sensor, plugins.end()); + EXPECT_EQ(it_sensor->second.order, 0); + EXPECT_EQ(it_sensor->second.params.at("noise"), "0.1"); + + for (const auto& [key, plugin] : plugins) { + EXPECT_EQ(plugin.entity_name, "entity_common_stack"); + EXPECT_EQ(plugin.entity_tag, "common_entity"); + EXPECT_EQ(plugin.entity_block_id, 0); + } +} + +TEST(MissionParseStandaloneTest, entity_local_motion_model_overrides_entity_common_motion_model) { + scrimmage::MissionParse mp; + ASSERT_TRUE(mp.parse("missions/test/test_entity_common_motion_override.xml")); + + const auto& plugins = get_entity_plugins(mp, 0); + EXPECT_EQ(plugins.count("motion_model:SimpleAircraft"), 0); + + auto it_motion = plugins.find("motion_model:Ballistic"); + ASSERT_NE(it_motion, plugins.end()); + EXPECT_EQ(it_motion->second.order, 0); + + auto motion_plugins = mp.get_plugins_by_type(0, "motion_model"); + ASSERT_EQ(motion_plugins.size(), 1); + EXPECT_EQ(motion_plugins.front().name, "Ballistic"); + EXPECT_EQ(motion_plugins.front().params.at("max_speed"), "30"); +} + INSTANTIATE_TEST_SUITE_P( MissionParsingTestSuite, MissionParseTest, diff --git a/test/test_mission_validation.cpp b/test/test_mission_validation.cpp new file mode 100644 index 0000000000..230f6963c1 --- /dev/null +++ b/test/test_mission_validation.cpp @@ -0,0 +1,55 @@ +/*! + * @file + */ + +#include + +#include "scrimmage/common/FileSearch.h" +#include "scrimmage/parse/MissionParse.h" +#include "scrimmage/parse/MissionValidation.h" + +namespace { + +const std::map& get_entity_plugins( + const scrimmage::MissionParse& mission_parse, int entity_block_id) { + static const std::map empty_plugins; + const auto& all_plugins = mission_parse.all_entity_plugins(); + auto plugin_it = all_plugins.find(entity_block_id); + EXPECT_NE(plugin_it, all_plugins.end()); + if (plugin_it == all_plugins.end()) { + return empty_plugins; + } + return plugin_it->second; +} + +} // namespace + +TEST(MissionValidationTest, diagnostics_are_stored_in_validation_result) { + auto mp = std::make_shared(); + ASSERT_TRUE(mp->parse("missions/test-duplicate-invalid-plugin.xml")); + + scrimmage::FileSearch file_search; + scrimmage::MissionValidation validator; + + auto result = validator.validate(mp, file_search); + + EXPECT_FALSE(result.valid()); + ASSERT_EQ(result.errors.size(), 2); + + auto entity_it = result.entity_plugin_diagnostics.find(0); + ASSERT_NE(entity_it, result.entity_plugin_diagnostics.end()); + + auto first_it = entity_it->second.find("autonomy:Straigt"); + ASSERT_NE(first_it, entity_it->second.end()); + EXPECT_EQ(first_it->second.status, scrimmage::PluginValidationInfo::Status::config_not_found); + EXPECT_THAT(first_it->second.suggestions, testing::Contains("Straight")); + + auto second_it = entity_it->second.find("autonomy:Straigt:1"); + ASSERT_NE(second_it, entity_it->second.end()); + EXPECT_EQ(second_it->second.status, scrimmage::PluginValidationInfo::Status::config_not_found); + EXPECT_THAT(second_it->second.suggestions, testing::Contains("Straight")); + + const auto& plugins = get_entity_plugins(*mp, 0); + ASSERT_NE(plugins.find("autonomy:Straigt"), plugins.end()); + ASSERT_NE(plugins.find("autonomy:Straigt:1"), plugins.end()); +} \ No newline at end of file