From fc9fdc1bbeb02f352bd05edeedbf6128d3a12a90 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 6 Mar 2025 12:16:46 +0900 Subject: [PATCH 1/2] Unify spawn API --- .../simulator_core.hpp | 2 +- .../include/traffic_simulator/api/api.hpp | 136 +++++++----------- .../entity/entity_manager.hpp | 4 +- 3 files changed, 55 insertions(+), 87 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index d79ddbd789f..27d79524451 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -300,7 +300,7 @@ class SimulatorCore } template - static auto applyAddEntityAction(Ts &&... xs) + static auto applyAddEntityAction(Ts &&... xs) -> decltype(auto) { return core->spawn(std::forward(xs)...); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index be76bfc48b5..f08a79b1de4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -106,108 +106,76 @@ class API auto updateFrame() -> bool; // entities, ego - spawn - template + template < + typename PoseType, typename ParamsType, + typename = std::enable_if_t, traffic_simulator_msgs::msg::VehicleParameters>, + std::is_same, traffic_simulator_msgs::msg::PedestrianParameters>, + std::is_same, traffic_simulator_msgs::msg::MiscObjectParameters>>>> auto spawn( - const std::string & name, const Pose & pose, - const traffic_simulator_msgs::msg::VehicleParameters & parameters, - const std::string & behavior = VehicleBehavior::defaultBehavior(), - const std::string & model3d = "") + const std::string & name, const PoseType & pose, const ParamsType & parameters, + const std::string & behavior = "", const std::string & model3d = "") -> entity::EntityBase & { - auto register_to_entity_manager = [&]() { + using VehicleParameters = traffic_simulator_msgs::msg::VehicleParameters; + using PedestrianParameters = traffic_simulator_msgs::msg::PedestrianParameters; + using MiscObjectParameters = traffic_simulator_msgs::msg::MiscObjectParameters; + + auto register_to_entity_manager = [&]() -> entity::EntityBase & { + if constexpr (std::is_same_v) { if (behavior == VehicleBehavior::autoware()) { - return entity_manager_ptr_->isEntityExist(name) or - entity_manager_ptr_->spawnEntity( + return entity_manager_ptr_->spawnEntity( name, pose, parameters, getCurrentTime(), configuration_, node_parameters_); } else { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, getCurrentTime(), behavior); - } - }; - - auto register_to_environment_simulator = [&]() { - if (configuration_.standalone_mode) { - return true; - } else if (!entity_manager_ptr_->isEntityExist(name)) { - throw common::SemanticError( - "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); - } else { - simulation_api_schema::SpawnVehicleEntityRequest req; - simulation_interface::toProto(parameters, *req.mutable_parameters()); - req.mutable_parameters()->set_name(name); - req.set_asset_key(model3d); - simulation_interface::toProto( - entity_manager_ptr_->getEntity(name).getMapPose(), *req.mutable_pose()); - req.set_is_ego(behavior == VehicleBehavior::autoware()); - /// @todo Should be filled from function API - req.set_initial_speed(0.0); - return zeromq_client_.call(req).result().success(); + name, pose, parameters, getCurrentTime(), + behavior.empty() ? VehicleBehavior::defaultBehavior() : behavior); + } + } else if constexpr (std::is_same_v) { + return entity_manager_ptr_->spawnEntity( + name, pose, parameters, getCurrentTime(), + behavior.empty() ? PedestrianBehavior::defaultBehavior() : behavior); + } else if constexpr (std::is_same_v) { + return entity_manager_ptr_->spawnEntity( + name, pose, parameters, getCurrentTime()); } }; - return register_to_entity_manager() and register_to_environment_simulator(); - } - - template - auto spawn( - const std::string & name, const PoseType & pose, - const traffic_simulator_msgs::msg::PedestrianParameters & parameters, - const std::string & behavior = PedestrianBehavior::defaultBehavior(), - const std::string & model3d = "") - { - auto register_to_entity_manager = [&]() { - return entity_manager_ptr_->spawnEntity( - name, pose, parameters, getCurrentTime(), behavior); + auto prepare_and_send_request = [&](const auto & entity, auto & request) -> bool { + simulation_interface::toProto(parameters, *request.mutable_parameters()); + request.mutable_parameters()->set_name(name); + request.set_asset_key(model3d); + simulation_interface::toProto(entity.getMapPose(), *request.mutable_pose()); + return zeromq_client_.call(request).result().success(); }; - auto register_to_environment_simulator = [&]() { + auto register_to_environment_simulator = [&](const auto & entity) -> bool { if (configuration_.standalone_mode) { return true; - } else if (!entity_manager_ptr_->isEntityExist(name)) { - throw common::SemanticError( - "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); } else { - simulation_api_schema::SpawnPedestrianEntityRequest req; - simulation_interface::toProto(parameters, *req.mutable_parameters()); - req.mutable_parameters()->set_name(name); - req.set_asset_key(model3d); - simulation_interface::toProto( - entity_manager_ptr_->getEntity(name).getMapPose(), *req.mutable_pose()); - return zeromq_client_.call(req).result().success(); + if constexpr (std::is_same_v) { + simulation_api_schema::SpawnVehicleEntityRequest request; + request.set_is_ego(behavior == VehicleBehavior::autoware()); + /// @todo Should be filled from function API + request.set_initial_speed(0.0); + return prepare_and_send_request(entity, request); + } else if constexpr (std::is_same_v) { + simulation_api_schema::SpawnPedestrianEntityRequest request; + return prepare_and_send_request(entity, request); + } else if constexpr (std::is_same_v) { + simulation_api_schema::SpawnMiscObjectEntityRequest request; + return prepare_and_send_request(entity, request); + } else { + return false; + } } }; - return register_to_entity_manager() and register_to_environment_simulator(); - } - - template - auto spawn( - const std::string & name, const PoseType & pose, - const traffic_simulator_msgs::msg::MiscObjectParameters & parameters, - const std::string & model3d = "") - { - auto register_to_entity_manager = [&]() { - return entity_manager_ptr_->spawnEntity( - name, pose, parameters, getCurrentTime()); - }; - - auto register_to_environment_simulator = [&]() { - if (configuration_.standalone_mode) { - return true; - } else if (!entity_manager_ptr_->isEntityExist(name)) { - throw common::SemanticError( - "Entity ", name, " can not be registered in simulator - it has not been spawned yet."); + auto & entity = register_to_entity_manager(); + if (register_to_environment_simulator(entity)) { + return entity; } else { - simulation_api_schema::SpawnMiscObjectEntityRequest req; - simulation_interface::toProto(parameters, *req.mutable_parameters()); - req.mutable_parameters()->set_name(name); - req.set_asset_key(model3d); - simulation_interface::toProto( - entity_manager_ptr_->getEntity(name).getMapPose(), *req.mutable_pose()); - return zeromq_client_.call(req).result().success(); - } - }; - - return register_to_entity_manager() and register_to_environment_simulator(); + THROW_SEMANTIC_ERROR("Spawn entity ", std::quoted(name), " resulted in failure."); + } } // sensors - attach diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 7e7f8c32eb3..1bdb3674721 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -106,7 +106,7 @@ class EntityManager template auto spawnEntity( const std::string & name, const PoseType & pose, const ParametersType & parameters, - const double current_time, Ts &&... xs) + const double current_time, Ts &&... xs) -> entity::EntityBase & { static_assert( std::disjunction< @@ -179,7 +179,7 @@ class EntityManager success) { // FIXME: this ignores V2I traffic lights iter->second->setTrafficLights(traffic_lights_ptr_->getConventionalTrafficLights()); - return success; + return *(iter->second); } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); } From 473a98874f55cd736a6c226d8ecf7e4351aa8f75 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 6 Mar 2025 14:02:16 +0900 Subject: [PATCH 2/2] Apply formatter --- .../include/traffic_simulator/api/api.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index f08a79b1de4..d25b34b3732 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -122,11 +122,11 @@ class API auto register_to_entity_manager = [&]() -> entity::EntityBase & { if constexpr (std::is_same_v) { - if (behavior == VehicleBehavior::autoware()) { + if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, getCurrentTime(), configuration_, node_parameters_); - } else { - return entity_manager_ptr_->spawnEntity( + name, pose, parameters, getCurrentTime(), configuration_, node_parameters_); + } else { + return entity_manager_ptr_->spawnEntity( name, pose, parameters, getCurrentTime(), behavior.empty() ? VehicleBehavior::defaultBehavior() : behavior); } @@ -173,7 +173,7 @@ class API auto & entity = register_to_entity_manager(); if (register_to_environment_simulator(entity)) { return entity; - } else { + } else { THROW_SEMANTIC_ERROR("Spawn entity ", std::quoted(name), " resulted in failure."); } }