Feature/reset behavior plugin

mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp

@@ -49,6 +49,22 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode
       api_.getCurrentAction("pedestrian") != "do_nothing") {
       stop(cpp_mock_scenarios::Result::FAILURE);
     }
+    if (
+      api_.getCurrentAction("vehicle_spawn_with_behavior_tree") == "do_nothing" ||
+      api_.getCurrentAction("pedestrian_spawn_with_behavior_tree") == "do_nothing") {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
+    api_.resetBehaviorPlugin(
+      "vehicle_spawn_with_behavior_tree",
+      traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing());
+    api_.resetBehaviorPlugin(
+      "pedestrian_spawn_with_behavior_tree",
+      traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing());
+    if (
+      api_.getCurrentAction("vehicle_spawn_with_behavior_tree") != "do_nothing" ||
+      api_.getCurrentAction("pedestrian_spawn_with_behavior_tree") != "do_nothing") {
+      stop(cpp_mock_scenarios::Result::FAILURE);
+    }
 
     stop(cpp_mock_scenarios::Result::SUCCESS);
   }
@@ -62,6 +78,16 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode
       "pedestrian", api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34741, 3, 0)),
       getPedestrianParameters(),
       traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing());
+    api_.spawn(
+      "vehicle_spawn_with_behavior_tree",
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34741, 2.0, 0)),
+      getVehicleParameters(),
+      traffic_simulator::entity::VehicleEntity::BuiltinBehavior::behaviorTree());
+    api_.spawn(
+      "pedestrian_spawn_with_behavior_tree",
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34741, 3, 0)),
+      getPedestrianParameters(),
+      traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::behaviorTree());
   }
 };
 }  // namespace cpp_mock_scenarios

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -343,6 +343,7 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory);
   FORWARD_TO_ENTITY_MANAGER(requestSpeedChange);
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
+  FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(setAcceleration);

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -229,6 +229,8 @@ class EntityBase
 
   /*   */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void;
 
+  /*   */ auto setLinearJerk(const double liner_jerk) -> void;
+
   virtual void startNpcLogic();
 
   /*   */ void stopAtCurrentPosition();

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

@@ -289,6 +289,7 @@ class EntityManager
   FORWARD_TO_ENTITY(getCurrentAccel, const);
   FORWARD_TO_ENTITY(getCurrentAction, const);
   FORWARD_TO_ENTITY(getCurrentTwist, const);
+  FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const);
   FORWARD_TO_ENTITY(getDistanceToLaneBound, );
   FORWARD_TO_ENTITY(getDistanceToLaneBound, const);
   FORWARD_TO_ENTITY(getDistanceToLeftLaneBound, );
@@ -297,20 +298,20 @@ class EntityManager
   FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const);
   FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const);
   FORWARD_TO_ENTITY(getEntityType, const);
+  FORWARD_TO_ENTITY(getEntityTypename, const);
   FORWARD_TO_ENTITY(getLaneletPose, const);
   FORWARD_TO_ENTITY(getLinearJerk, const);
   FORWARD_TO_ENTITY(getMapPose, const);
   FORWARD_TO_ENTITY(getMapPoseFromRelativePose, const);
   FORWARD_TO_ENTITY(getRouteLanelets, const);
   FORWARD_TO_ENTITY(getStandStillDuration, const);
   FORWARD_TO_ENTITY(getTraveledDistance, const);
-  FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const);
   FORWARD_TO_ENTITY(laneMatchingSucceed, const);
   FORWARD_TO_ENTITY(activateOutOfRangeJob, );
   FORWARD_TO_ENTITY(cancelRequest, );
+  FORWARD_TO_ENTITY(isControlledBySimulator, );
   FORWARD_TO_ENTITY(requestAcquirePosition, );
   FORWARD_TO_ENTITY(requestAssignRoute, );
-  FORWARD_TO_ENTITY(isControlledBySimulator, );
   FORWARD_TO_ENTITY(requestFollowTrajectory, );
   FORWARD_TO_ENTITY(requestLaneChange, );
   FORWARD_TO_ENTITY(requestWalkStraight, );
@@ -320,6 +321,7 @@ class EntityManager
   FORWARD_TO_ENTITY(setBehaviorParameter, );
   FORWARD_TO_ENTITY(setDecelerationLimit, );
   FORWARD_TO_ENTITY(setDecelerationRateLimit, );
+  FORWARD_TO_ENTITY(setLinearJerk, );
   FORWARD_TO_ENTITY(setLinearVelocity, );
   FORWARD_TO_ENTITY(setMapPose, );
   FORWARD_TO_ENTITY(setTwist, );
@@ -417,6 +419,9 @@ class EntityManager
   auto getObstacle(const std::string & name)
     -> std::optional<traffic_simulator_msgs::msg::Obstacle>;
 
+  auto getPedestrianParameters(const std::string & name) const
+    -> const traffic_simulator_msgs::msg::PedestrianParameters &;
+
   // clang-format off
   auto getRelativePose(const geometry_msgs::msg::Pose     & from, const geometry_msgs::msg::Pose     & to) const -> geometry_msgs::msg::Pose;
   auto getRelativePose(const geometry_msgs::msg::Pose     & from, const std::string                  & to) const -> geometry_msgs::msg::Pose;
@@ -430,6 +435,9 @@ class EntityManager
 
   auto getStepTime() const noexcept -> double;
 
+  auto getVehicleParameters(const std::string & name) const
+    -> const traffic_simulator_msgs::msg::VehicleParameters &;
+
   auto getWaypoints(const std::string & name) -> traffic_simulator_msgs::msg::WaypointsArray;
 
   template <typename T>
@@ -454,7 +462,11 @@ class EntityManager
     }
   }
 
-  bool isEgo(const std::string & name) const;
+  template <typename EntityType>
+  bool is(const std::string & name) const
+  {
+    return dynamic_cast<EntityType const *>(entities_.at(name).get()) != nullptr;
+  }
 
   bool isEgoSpawned() const;
 
@@ -476,6 +488,16 @@ class EntityManager
   void requestLaneChange(
     const std::string & name, const traffic_simulator::lane_change::Direction & direction);
 
+  /**
+   * @brief Reset behavior plugin of the target entity.
+   * The internal behavior is to take over the various parameters and save them, then respawn the Entity and set the parameters.
+   * @param name The name of the target entity.
+   * @param behavior_plugin_name The name of the behavior plugin you want to set.
+   * @sa traffic_simulator::entity::PedestrianEntity::BuiltinBehavior
+   * @sa traffic_simulator::entity::VehicleEntity::BuiltinBehavior
+   */
+  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);
@@ -490,7 +512,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 {
@@ -563,7 +585,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<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<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_base.cpp

@@ -759,6 +759,13 @@ auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> voi
   status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
 }
 
+auto EntityBase::setLinearJerk(const double linear_jerk) -> void
+{
+  auto new_status = static_cast<EntityStatus>(getStatus());
+  new_status.action_status.linear_jerk = linear_jerk;
+  status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
+}
+
 auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void
 {
   THROW_SEMANTIC_ERROR(

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;
     }
   }
@@ -513,6 +513,17 @@ auto EntityManager::getObstacle(const std::string & name)
   return entities_.at(name)->getObstacle();
 }
 
+auto EntityManager::getPedestrianParameters(const std::string & name) const
+  -> const traffic_simulator_msgs::msg::PedestrianParameters &
+{
+  if (const auto entity = dynamic_cast<PedestrianEntity const *>(entities_.at(name).get())) {
+    return entity->pedestrian_parameters;
+  }
+  THROW_SIMULATION_ERROR(
+    "EntityType: ", getEntityTypename(name), ", does not have pedestrian parameter.",
+    "Please check description of the scenario and entity type of the Entity: " + name);
+}
+
 auto EntityManager::getRelativePose(
   const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) const
   -> geometry_msgs::msg::Pose
@@ -566,6 +577,17 @@ auto EntityManager::getRelativePose(
 
 auto EntityManager::getStepTime() const noexcept -> double { return step_time_; }
 
+auto EntityManager::getVehicleParameters(const std::string & name) const
+  -> const traffic_simulator_msgs::msg::VehicleParameters &
+{
+  if (const auto vehicle = dynamic_cast<VehicleEntity const *>(entities_.at(name).get())) {
+    return vehicle->vehicle_parameters;
+  }
+  THROW_SIMULATION_ERROR(
+    "EntityType: ", getEntityTypename(name), ", does not have pedestrian parameter.",
+    "Please check description of the scenario and entity type of the Entity: " + name);
+}
+
 auto EntityManager::getWaypoints(const std::string & name)
   -> traffic_simulator_msgs::msg::WaypointsArray
 {
@@ -575,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;
     }
   }
@@ -654,6 +669,36 @@ void EntityManager::requestLaneChange(
   }
 }
 
+void EntityManager::resetBehaviorPlugin(
+  const std::string & name, const std::string & behavior_plugin_name)
+{
+  const auto status = getEntityStatus(name);
+  const auto behavior_parameter = getBehaviorParameter(name);
+  if (is<EgoEntity>(name)) {
+    THROW_SEMANTIC_ERROR(
+      "Entity :", name, "is EgoEntity.", "You cannot reset behavior plugin of EgoEntity.");
+  } else if (is<MiscObjectEntity>(name)) {
+    THROW_SEMANTIC_ERROR(
+      "Entity :", name, "is MiscObjectEntity.",
+      "You cannot reset behavior plugin of MiscObjectEntity.");
+  } else if (is<VehicleEntity>(name)) {
+    const auto parameters = getVehicleParameters(name);
+    despawnEntity(name);
+    spawnEntity<VehicleEntity>(name, status.getMapPose(), parameters, behavior_plugin_name);
+  } else if (is<PedestrianEntity>(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.");
+  }
+  setLinearJerk(name, status.getLinearJerk());
+  setAcceleration(name, status.getAccel());
+  setTwist(name, status.getTwist());
+  setBehaviorParameter(name, behavior_parameter);
+}
+
 bool EntityManager::trafficLightsChanged()
 {
   return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or
@@ -663,7 +708,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);
@@ -673,7 +718,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);
@@ -682,7 +727,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);
@@ -693,7 +738,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);
@@ -702,7 +747,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.");