From 1172d2888b05b032da1a37adbe42301a03aabf66 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 7 Mar 2024 17:43:35 +0900 Subject: [PATCH] add resetBehaviorPlugin function Signed-off-by: Masaya Kataoka --- .../traffic_simulator/entity/entity_base.hpp | 4 ++ .../entity/entity_manager.hpp | 10 ++++ .../entity/pedestrian_entity.hpp | 2 + .../entity/vehicle_entity.hpp | 2 + .../src/entity/entity_base.cpp | 14 ++++++ .../src/entity/entity_manager.cpp | 47 +++++++++++++++++++ .../src/entity/pedestrian_entity.cpp | 6 +++ .../src/entity/vehicle_entity.cpp | 5 ++ 8 files changed, 90 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index a089fc77584..2034de11ebd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -141,8 +141,12 @@ class EntityBase virtual auto getObstacle() -> std::optional = 0; + virtual auto getPedestrianParameters() -> traffic_simulator_msgs::msg::PedestrianParameters; + virtual auto getRouteLanelets(double horizon = 100) -> lanelet::Ids = 0; + virtual auto getVehicleParameters() -> traffic_simulator_msgs::msg::VehicleParameters; + virtual auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void = 0; virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0; 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 9a7d3029d1b..37dfeed5ec0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -301,9 +301,11 @@ class EntityManager FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getMapPose, const); FORWARD_TO_ENTITY(getMapPoseFromRelativePose, const); + FORWARD_TO_ENTITY(getPedestrianParameters, const); FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); + FORWARD_TO_ENTITY(getVehicleParameters, const); FORWARD_TO_ENTITY(laneMatchingSucceed, const); FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(cancelRequest, ); @@ -457,6 +459,12 @@ class EntityManager bool isEgoSpawned() const; + bool isVehicle(const std::string & name) const; + + bool isPedestrian(const std::string & name) const; + + bool isMiscObject(const std::string & name) const; + const std::string getEgoName() const; bool isInLanelet(const std::string & name, const lanelet::Id lanelet_id, const double tolerance); @@ -475,6 +483,8 @@ class EntityManager void requestLaneChange( const std::string & name, const traffic_simulator::lane_change::Direction & direction); + void resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name); + auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; void setVerbose(const bool verbose); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index 9c4ad92f3fe..a2f202d3938 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -111,6 +111,8 @@ class PedestrianEntity : public EntityBase auto getObstacle() -> std::optional override; + auto getPedestrianParameters() -> traffic_simulator_msgs::msg::PedestrianParameters override; + auto getGoalPoses() -> std::vector override; auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index 5b75362db12..ca54a39bdee 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -90,6 +90,8 @@ class VehicleEntity : public EntityBase auto getRouteLanelets(double horizon = 100) -> lanelet::Ids override; + auto getVehicleParameters() -> traffic_simulator_msgs::msg::VehicleParameters override; + auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; void onUpdate(double current_time, double step_time) override; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 6bbdf68167b..9985d631cb0 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -259,6 +259,20 @@ auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> return getBoundingBox().dimensions.y * 0.5 + 1.0; } +auto EntityBase::getPedestrianParameters() -> traffic_simulator_msgs::msg::PedestrianParameters +{ + THROW_SEMANTIC_ERROR( + "getPedestrianParameters function only supports pedestrian entity.", + "Please check the type of the entity : ", name); +} + +auto EntityBase::getVehicleParameters() -> traffic_simulator_msgs::msg::VehicleParameters +{ + THROW_SEMANTIC_ERROR( + "getVehicleParameters function only supports vehicle entity.", + "Please check the type of the entity : ", name); +} + auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool { return speed_planner_->isTargetSpeedReached(target_speed, getCurrentTwist()); diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 0fc5f2ecd73..0c324fde3bf 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -592,6 +592,27 @@ bool EntityManager::isEgoSpawned() const return false; } +bool EntityManager::isVehicle(const std::string & name) const +{ + using traffic_simulator_msgs::msg::EntityType; + return getEntityType(name).type == EntityType::VEHICLE and + dynamic_cast(entities_.at(name).get()); +} + +bool EntityManager::isPedestrian(const std::string & name) const +{ + using traffic_simulator_msgs::msg::EntityType; + return getEntityType(name).type == EntityType::PEDESTRIAN and + dynamic_cast(entities_.at(name).get()); +} + +bool EntityManager::isMiscObject(const std::string & name) const +{ + using traffic_simulator_msgs::msg::EntityType; + return getEntityType(name).type == EntityType::MISC_OBJECT and + dynamic_cast(entities_.at(name).get()); +} + bool EntityManager::isInLanelet( const std::string & name, const lanelet::Id lanelet_id, const double tolerance) { @@ -654,6 +675,32 @@ void EntityManager::requestLaneChange( } } +void EntityManager::resetBehaviorPlugin( + const std::string & name, const std::string & behavior_plugin_name) +{ + if (isEgo(name)) { + THROW_SEMANTIC_ERROR( + "Entity :", name, "is EgoEitity.", "You cannot reset behavior plugin of EgoEntity."); + } else if (isMiscObject(name)) { + THROW_SEMANTIC_ERROR( + "Entity :", name, "is MiscObjectEitity.", + "You cannot reset behavior plugin of MiscObjectEntity."); + } else if (isVehicle(name)) { + const auto status = getEntityStatus(name); + const auto parameters = getVehicleParameters(name); + despawnEntity(name); + spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + } else if (isPedestrian(name)) { + const auto status = getEntityStatus(name); + const auto parameters = getPedestrianParameters(name); + despawnEntity(name); + spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + } else { + THROW_SIMULATION_ERROR( + "Entity :", name, "is unkown entity type.", "Please contact to developer."); + } +} + bool EntityManager::trafficLightsChanged() { return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index c4f02ff3694..530c2077f21 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -121,6 +121,12 @@ auto PedestrianEntity::getObstacle() -> std::optional traffic_simulator_msgs::msg::PedestrianParameters +{ + return pedestrian_parameters; +} + auto PedestrianEntity::getGoalPoses() -> std::vector { return route_planner_.getGoalPoses(); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index 18568ecb05b..793beab88c1 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -121,6 +121,11 @@ auto VehicleEntity::getRouteLanelets(double horizon) -> lanelet::Ids } } +auto VehicleEntity::getVehicleParameters() -> traffic_simulator_msgs::msg::VehicleParameters +{ + return vehicle_parameters; +} + auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { if (!npc_logic_started_) {