enable reset behavior parameter

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

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::PedestrianEntity::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,14 @@ 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());
+    api_.spawn(
+      "pedestrian_spawn_with_behavior_tree",
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34741, 3, 0)),
+      getPedestrianParameters());
   }
 };
 }  // 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/src/entity/entity_manager.cpp

@@ -678,6 +678,8 @@ 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 (isEgo(name)) {
     THROW_SEMANTIC_ERROR(
       "Entity :", name, "is EgoEitity.", "You cannot reset behavior plugin of EgoEntity.");
@@ -686,19 +688,20 @@ void EntityManager::resetBehaviorPlugin(
       "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.");
   }
+  setAcceleration(name, status.getAccel());
+  setTwist(name, status.getTwist());
+  setBehaviorParameter(name, behavior_parameter);
 }
 
 bool EntityManager::trafficLightsChanged()