add resetBehaviorPlugin function

Signed-off-by: Masaya Kataoka <[email protected]>
tier4 · Mar 7, 2024 · 1172d28 · 1172d28
1 parent 7ef5cdc
commit 1172d28
Show file tree

Hide file tree

Showing 8 changed files with 90 additions and 0 deletions.
diff --git 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<traffic_simulator_msgs::msg::Obstacle> = 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
@@ -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
@@ -111,6 +111,8 @@ class PedestrianEntity : public EntityBase
 
   auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> override;
 
+  auto getPedestrianParameters() -> traffic_simulator_msgs::msg::PedestrianParameters override;
+
   auto getGoalPoses() -> std::vector<CanonicalizedLaneletPose> 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
@@ -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
@@ -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
@@ -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<VehicleEntity const *>(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<PedestrianEntity const *>(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<MiscObjectEntity const *>(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<VehicleEntity>(name, status.getMapPose(), parameters, behavior_plugin_name);
+  } else if (isPedestrian(name)) {
+    const auto status = getEntityStatus(name);
+    const auto parameters = getPedestrianParameters(name);
+    despawnEntity(name);
+    spawnEntity<PedestrianEntity>(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
@@ -121,6 +121,12 @@ auto PedestrianEntity::getObstacle() -> std::optional<traffic_simulator_msgs::ms
   return std::nullopt;
 }
 
+auto PedestrianEntity::getPedestrianParameters()
+  -> traffic_simulator_msgs::msg::PedestrianParameters
+{
+  return pedestrian_parameters;
+}
+
 auto PedestrianEntity::getGoalPoses() -> std::vector<CanonicalizedLaneletPose>
 {
   return route_planner_.getGoalPoses();

diff --git 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_) {