remove isEgo function and add is<EntityType> function

Signed-off-by: Masaya Kataoka <[email protected]>

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -461,6 +461,12 @@ class EntityManager
     }
   }
 
+  template <typename EntityType>
+  bool is(const std::string & name) const
+  {
+    return dynamic_cast<EntityType const *>(entities_.at(name).get());
+  }
+
   bool isEgo(const std::string & name) const;
 
   bool isEgoSpawned() const;
@@ -513,7 +519,7 @@ class EntityManager
       if constexpr (std::is_same_v<std::decay_t<Entity>, EgoEntity>) {
         if (auto iter = std::find_if(
               std::begin(entities_), std::end(entities_),
-              [this](auto && each) { return isEgo(each.first); });
+              [this](auto && each) { return is<EgoEntity>(each.first); });
             iter != std::end(entities_)) {
           THROW_SEMANTIC_ERROR("multi ego simulation does not support yet");
         } else {
@@ -586,7 +592,7 @@ class EntityManager
         success) {
       // FIXME: this ignores V2I traffic lights
       iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_);
-      if (npc_logic_started_ && not isEgo(name)) {
+      if (npc_logic_started_ && not is<EgoEntity>(name)) {
         iter->second->startNpcLogic();
       }
       return success;

simulation/traffic_simulator/src/api/api.cpp

@@ -228,7 +228,7 @@ bool API::updateEntitiesStatusInSim()
   for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) {
     auto entity_status = entity_manager_ptr_->getEntityStatus(entity_name);
     simulation_interface::toProto(static_cast<EntityStatus>(entity_status), *req.add_status());
-    if (entity_manager_ptr_->isEgo(entity_name)) {
+    if (entity_manager_ptr_->is<traffic_simulator::entity::EgoEntity>(entity_name)) {
       req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name));
     }
   }
@@ -241,7 +241,7 @@ bool API::updateEntitiesStatusInSim()
       simulation_interface::toMsg(res_status.pose(), entity_status.pose);
       simulation_interface::toMsg(res_status.action_status(), entity_status.action_status);
 
-      if (entity_manager_ptr_->isEgo(name)) {
+      if (entity_manager_ptr_->is<traffic_simulator::entity::EgoEntity>(name)) {
         setMapPose(name, entity_status.pose);
         setTwist(name, entity_status.action_status.twist);
         setAcceleration(name, entity_status.action_status.accel);

simulation/traffic_simulator/src/entity/entity_manager.cpp

@@ -487,15 +487,15 @@ auto EntityManager::getLongitudinalDistance(
 auto EntityManager::getNumberOfEgo() const -> std::size_t
 {
   return std::count_if(std::begin(entities_), std::end(entities_), [this](const auto & each) {
-    return isEgo(each.first);
+    return is<EgoEntity>(each.first);
   });
 }
 
 const std::string EntityManager::getEgoName() const
 {
   const auto names = getEntityNames();
   for (const auto & name : names) {
-    if (isEgo(name)) {
+    if (is<EgoEntity>(name)) {
       return name;
     }
   }
@@ -597,17 +597,10 @@ auto EntityManager::getWaypoints(const std::string & name)
   return entities_.at(name)->getWaypoints();
 }
 
-bool EntityManager::isEgo(const std::string & name) const
-{
-  using traffic_simulator_msgs::msg::EntityType;
-  return getEntityType(name).type == EntityType::EGO and
-         dynamic_cast<EgoEntity const *>(entities_.at(name).get());
-}
-
 bool EntityManager::isEgoSpawned() const
 {
   for (const auto & name : getEntityNames()) {
-    if (isEgo(name)) {
+    if (is<EgoEntity>(name)) {
       return true;
     }
   }
@@ -702,7 +695,7 @@ void EntityManager::resetBehaviorPlugin(
 {
   const auto status = getEntityStatus(name);
   const auto behavior_parameter = getBehaviorParameter(name);
-  if (isEgo(name)) {
+  if (is<EgoEntity>(name)) {
     THROW_SEMANTIC_ERROR(
       "Entity :", name, "is EgoEntity.", "You cannot reset behavior plugin of EgoEntity.");
   } else if (isMiscObject(name)) {
@@ -735,7 +728,7 @@ bool EntityManager::trafficLightsChanged()
 void EntityManager::requestSpeedChange(
   const std::string & name, double target_speed, bool continuous)
 {
-  if (isEgo(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, continuous);
@@ -745,7 +738,7 @@ void EntityManager::requestSpeedChange(
   const std::string & name, const double target_speed, const speed_change::Transition transition,
   const speed_change::Constraint constraint, const bool continuous)
 {
-  if (isEgo(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous);
@@ -754,7 +747,7 @@ void EntityManager::requestSpeedChange(
 void EntityManager::requestSpeedChange(
   const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
 {
-  if (isEgo(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, continuous);
@@ -765,7 +758,7 @@ void EntityManager::requestSpeedChange(
   const speed_change::Transition transition, const speed_change::Constraint constraint,
   const bool continuous)
 {
-  if (isEgo(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous);
@@ -774,7 +767,7 @@ void EntityManager::requestSpeedChange(
 auto EntityManager::setEntityStatus(
   const std::string & name, const CanonicalizedEntityStatus & status) -> void
 {
-  if (isEgo(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
     THROW_SEMANTIC_ERROR(
       "You cannot set entity status to the ego vehicle name ", std::quoted(name),
       " after starting scenario.");