Feature/respawn entity

external/concealer/include/concealer/field_operator_application.hpp

@@ -127,6 +127,8 @@ class FieldOperatorApplication : public rclcpp::Node
    * -------------------------------------------------------------------------- */
   virtual auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void = 0;
 
+  virtual auto clearRoute() -> void = 0;
+
   virtual auto getAutowareStateName() const -> std::string = 0;
 
   virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0;

external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp

@@ -20,6 +20,7 @@
 #endif
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
@@ -64,6 +65,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   SubscriberWrapper<tier4_planning_msgs::msg::Trajectory>                         getTrajectory;
   SubscriberWrapper<autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand>       getTurnIndicatorsCommandImpl;
 
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ClearRoute>                  requestClearRoute;
   ServiceWithValidation<tier4_rtc_msgs::srv::CooperateCommands>                   requestCooperateCommands;
   ServiceWithValidation<tier4_external_api_msgs::srv::Engage>                     requestEngage;
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::InitializeLocalization>      requestInitialPose;
@@ -120,16 +122,17 @@ class FieldOperatorApplicationFor<AutowareUniverse>
     getLocalizationState("/api/localization/initialization_state", *this),
 #endif
     getMrmState("/api/fail_safe/mrm_state", *this, [this](const auto & v) { receiveMrmState(v); }),
+    getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this),
     getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", *this),
     getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", *this),
+    requestClearRoute("/api/routing/clear_route", *this),
     requestCooperateCommands("/api/external/set/rtc_commands", *this),
     requestEngage("/api/external/set/engage", *this),
     requestInitialPose("/api/localization/initialize", *this),
     // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons.
     requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)),
     requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this),
-    requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this),
-    getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this)
+    requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this)
   // clang-format on
   {
   }
@@ -159,6 +162,8 @@ class FieldOperatorApplicationFor<AutowareUniverse>
 
   auto plan(const std::vector<geometry_msgs::msg::PoseStamped> &) -> void override;
 
+  auto clearRoute() -> void override;
+
   auto requestAutoModeForCooperation(const std::string &, bool) -> void override;
 
   auto restrictTargetSpeed(double) const -> double override;

external/concealer/src/field_operator_application_for_autoware_universe.cpp

@@ -347,6 +347,13 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::plan(
   });
 }
 
+auto FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() -> void
+{
+  task_queue.delay([this] {
+    requestClearRoute(std::make_shared<autoware_adapi_v1_msgs::srv::ClearRoute::Request>());
+  });
+}
+
 auto FieldOperatorApplicationFor<AutowareUniverse>::engage() -> void
 {
   task_queue.delay([this]() {

mock/cpp_mock_scenarios/CMakeLists.txt

@@ -54,6 +54,8 @@ if(BUILD_CPP_MOCK_SCENARIOS)
   add_subdirectory(src/pedestrian)
   add_subdirectory(src/random_scenario)
   add_subdirectory(src/speed_planning)
+  # add_subdirectory(src/respawn_ego)
+
 endif()
 
 install(

mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp

@@ -93,6 +93,7 @@ void CppScenarioNode::spawnEgoEntity(
   api_.updateFrame();
   std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
   api_.spawn("ego", spawn_lanelet_pose, parameters, traffic_simulator::VehicleBehavior::autoware());
+  api_.asFieldOperatorApplication("ego").declare_parameter<bool>("allow_goal_modification", true);
   api_.attachLidarSensor("ego", 0.0);
 
   api_.attachDetectionSensor("ego", 200.0, true, 0.0, 0, 0.0, 0.0);
@@ -112,30 +113,15 @@ void CppScenarioNode::spawnEgoEntity(
     return configuration;
   }());
   api_.requestAssignRoute("ego", goal_lanelet_poses);
+
   using namespace std::chrono_literals;
-  std::atomic<bool> initialized(false);
-  auto initialize_thread = std::thread([&]() {
-    while (!api_.asFieldOperatorApplication("ego").engaged()) {
-      api_.updateFrame();
-      std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
-    }
-    initialized.store(true);
-  });
-  std::atomic<bool> engaged(false);
-  auto engage_thread = std::thread([&]() {
-    while (!api_.asFieldOperatorApplication("ego").engaged()) {
+  while (!api_.asFieldOperatorApplication("ego").engaged()) {
+    if (api_.asFieldOperatorApplication("ego").engageable()) {
       api_.asFieldOperatorApplication("ego").engage();
-      std::this_thread::sleep_for(1000ms);
     }
-    engaged.store(true);
-  });
-  while (!api_.asFieldOperatorApplication("ego").engaged() &&
-         !(initialized.load() && engaged.load())) {
-    // RCLCPP_INFO_STREAM(get_logger(), "Waiting for Autoware initialization...");
+    api_.updateFrame();
     std::this_thread::sleep_for(std::chrono::duration<double>(1.0 / 20.0));
   }
-  initialize_thread.join();
-  engage_thread.join();
 }
 
 void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration)

mock/cpp_mock_scenarios/src/respawn_ego/CMakeLists.txt

@@ -0,0 +1,14 @@
+ament_auto_add_executable(respawn_ego
+  respawn_ego.cpp
+)
+target_link_libraries(respawn_ego cpp_scenario_node)
+
+install(TARGETS
+  respawn_ego
+  DESTINATION lib/cpp_mock_scenarios
+)
+
+if(BUILD_TESTING)
+  include(../../cmake/add_cpp_mock_scenario_test.cmake)
+  add_cpp_mock_scenario_test(${PROJECT_NAME} "respawn_ego" "40.0")
+endif()

mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp

@@ -0,0 +1,78 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cpp_mock_scenarios/catalogs.hpp>
+#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <traffic_simulator/api/api.hpp>
+#include <vector>
+
+namespace cpp_mock_scenarios
+{
+class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode
+{
+public:
+  explicit RespawnEgoScenario(const rclcpp::NodeOptions & option)
+  : cpp_mock_scenarios::CppScenarioNode(
+      "respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
+      "lanelet2_map.osm", __FILE__, false, option),
+    goal_pose{
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34606, 0, 0, 0, 0, 0))},
+    new_position_subscriber{create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
+      [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) {
+        geometry_msgs::msg::PoseStamped goal_msg;
+        goal_msg.header.frame_id = "map";
+        goal_msg.pose = api_.toMapPose(goal_pose);
+        api_.respawn("ego", message, goal_msg);
+      })}
+  {
+    start();
+  }
+
+private:
+  void onUpdate() override
+  {
+    if (api_.getCurrentTime() >= 30) {
+      stop(cpp_mock_scenarios::Result::SUCCESS);
+    }
+  }
+
+  void onInitialize() override
+  {
+    spawnEgoEntity(
+      api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34621, 10, 0, 0, 0, 0)),
+      {goal_pose}, getVehicleParameters());
+  }
+
+private:
+  const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose goal_pose;
+
+  const rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_position_subscriber;
+};
+}  // namespace cpp_mock_scenarios
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  auto component = std::make_shared<cpp_mock_scenarios::RespawnEgoScenario>(options);
+  rclcpp::spin(component);
+  rclcpp::shutdown();
+  return 0;
+}

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -97,22 +97,29 @@ auto EgoEntitySimulation::makeSimulationModel(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters)
   -> const std::shared_ptr<SimModelInterface>
 {
+  auto node = rclcpp::Node("get_parameter", "simulation");
+
+  auto get_parameter = [&](const std::string & name, auto value = {}) {
+    node.declare_parameter<decltype(value)>(name, value);
+    node.get_parameter<decltype(value)>(name, value);
+    return value;
+  };
   // clang-format off
-  const auto acc_time_constant          = getParameter<double>("acc_time_constant",           0.1);
-  const auto acc_time_delay             = getParameter<double>("acc_time_delay",              0.1);
-  const auto acceleration_map_path      = getParameter<std::string>("acceleration_map_path",  "");
-  const auto debug_acc_scaling_factor   = getParameter<double>("debug_acc_scaling_factor",    1.0);
-  const auto debug_steer_scaling_factor = getParameter<double>("debug_steer_scaling_factor",  1.0);
-  const auto steer_lim                  = getParameter<double>("steer_lim",                   parameters.axles.front_axle.max_steering);  // 1.0
-  const auto steer_dead_band            = getParameter<double>("steer_dead_band",             0.0);
-  const auto steer_rate_lim             = getParameter<double>("steer_rate_lim",              5.0);
-  const auto steer_time_constant        = getParameter<double>("steer_time_constant",         0.27);
-  const auto steer_time_delay           = getParameter<double>("steer_time_delay",            0.24);
-  const auto vel_lim                    = getParameter<double>("vel_lim",                     parameters.performance.max_speed);  // 50.0
-  const auto vel_rate_lim               = getParameter<double>("vel_rate_lim",                parameters.performance.max_acceleration);  // 7.0
-  const auto vel_time_constant          = getParameter<double>("vel_time_constant",           0.1);  /// @note 0.5 is default value on simple_planning_simulator
-  const auto vel_time_delay             = getParameter<double>("vel_time_delay",              0.1);  /// @note 0.25 is default value on simple_planning_simulator
-  const auto wheel_base                 = getParameter<double>("wheel_base",                  parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
+  const auto acc_time_constant          = get_parameter("acc_time_constant",           0.1);
+  const auto acc_time_delay             = get_parameter("acc_time_delay",              0.1);
+  const auto acceleration_map_path      = get_parameter("acceleration_map_path",       std::string(""));
+  const auto debug_acc_scaling_factor   = get_parameter("debug_acc_scaling_factor",    1.0);
+  const auto debug_steer_scaling_factor = get_parameter("debug_steer_scaling_factor",  1.0);
+  const auto steer_lim                  = get_parameter("steer_lim",                   parameters.axles.front_axle.max_steering);  // 1.0
+  const auto steer_dead_band            = get_parameter("steer_dead_band",             0.0);
+  const auto steer_rate_lim             = get_parameter("steer_rate_lim",              5.0);
+  const auto steer_time_constant        = get_parameter("steer_time_constant",         0.27);
+  const auto steer_time_delay           = get_parameter("steer_time_delay",            0.24);
+  const auto vel_lim                    = get_parameter("vel_lim",                     parameters.performance.max_speed);  // 50.0
+  const auto vel_rate_lim               = get_parameter("vel_rate_lim",                parameters.performance.max_acceleration);  // 7.0
+  const auto vel_time_constant          = get_parameter("vel_time_constant",           0.1);  /// @note 0.5 is default value on simple_planning_simulator
+  const auto vel_time_delay             = get_parameter("vel_time_delay",              0.1);  /// @note 0.25 is default value on simple_planning_simulator
+  const auto wheel_base                 = get_parameter("wheel_base",                  parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
   // clang-format on
 
   switch (vehicle_model_type) {

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

@@ -221,6 +221,10 @@ class API
   bool despawn(const std::string & name);
   bool despawnEntities();
 
+  auto respawn(
+    const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
+    const geometry_msgs::msg::PoseStamped & goal_pose) -> void;
+
   auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void;
   auto setEntityStatus(
     const std::string & name, const geometry_msgs::msg::Pose & map_pose,

simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

@@ -112,6 +112,11 @@ class EgoEntity : public VehicleEntity
 
   auto isControlledBySimulator() const -> bool override;
 
+  auto setControlledBySimulator(bool state) -> void override
+  {
+    is_controlled_by_simulator_ = state;
+  }
+
   auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)
     -> void override;
 

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

@@ -186,6 +186,8 @@ class EntityBase
 
   virtual auto isControlledBySimulator() const -> bool;
 
+  virtual auto setControlledBySimulator(bool) -> void;
+
   virtual auto requestFollowTrajectory(
     const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void;
 

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

@@ -319,6 +319,7 @@ class EntityManager
   FORWARD_TO_ENTITY(setAccelerationLimit, );
   FORWARD_TO_ENTITY(setAccelerationRateLimit, );
   FORWARD_TO_ENTITY(setBehaviorParameter, );
+  FORWARD_TO_ENTITY(setControlledBySimulator, );
   FORWARD_TO_ENTITY(setDecelerationLimit, );
   FORWARD_TO_ENTITY(setDecelerationRateLimit, );
   FORWARD_TO_ENTITY(setLinearJerk, );

simulation/traffic_simulator/src/api/api.cpp

@@ -48,6 +48,54 @@ bool API::despawnEntities()
     entities.begin(), entities.end(), [&](const auto & entity) { return despawn(entity); });
 }
 
+auto API::respawn(
+  const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
+  const geometry_msgs::msg::PoseStamped & goal_pose) -> void
+{
+  if (not entity_manager_ptr_->is<entity::EgoEntity>(name)) {
+    throw std::runtime_error("Respawn of any entities other than EGO is not supported.");
+  } else if (new_pose.header.frame_id != "map") {
+    throw std::runtime_error("Respawn request with frame id other than map not supported.");
+  } else {
+    // set new pose and default action status in EntityManager
+    entity_manager_ptr_->setControlledBySimulator(name, true);
+    setEntityStatus(name, new_pose.pose.pose, helper::constructActionStatus());
+
+    // read status from EntityManager, then send it to SimpleSensorSimulator
+    simulation_api_schema::UpdateEntityStatusRequest req;
+    simulation_interface::toProto(
+      static_cast<EntityStatus>(entity_manager_ptr_->getEntityStatus(name)), *req.add_status());
+    req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted());
+    req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(name));
+    entity_manager_ptr_->setControlledBySimulator(name, false);
+
+    // check response
+    if (const auto res = zeromq_client_.call(req); not res.result().success()) {
+      throw common::SimulationError(
+        "UpdateEntityStatus request failed for \"" + name + "\" entity during respawn.");
+    } else if (const auto res_status = res.status().begin(); res.status().size() != 1) {
+      throw common::SimulationError(
+        "Failed to receive the new status of \"" + name + "\" entity after the update request.");
+    } else if (const auto res_name = res_status->name(); res_name != name) {
+      throw common::SimulationError(
+        "Wrong entity status received during respawn. Expected: \"" + name + "\". Received: \"" +
+        res_name + "\".");
+    } else {
+      // if valid, set response in EntityManager, then plan path and engage
+      auto entity_status = static_cast<EntityStatus>(entity_manager_ptr_->getEntityStatus(name));
+      simulation_interface::toMsg(res_status->pose(), entity_status.pose);
+      simulation_interface::toMsg(res_status->action_status(), entity_status.action_status);
+      setMapPose(name, entity_status.pose);
+      setTwist(name, entity_status.action_status.twist);
+      setAcceleration(name, entity_status.action_status.accel);
+
+      entity_manager_ptr_->asFieldOperatorApplication(name).clearRoute();
+      entity_manager_ptr_->asFieldOperatorApplication(name).plan({goal_pose});
+      entity_manager_ptr_->asFieldOperatorApplication(name).engage();
+    }
+  }
+}
+
 auto API::setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status)
   -> void
 {

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -656,6 +656,12 @@ void EntityBase::requestSpeedChange(
 
 auto EntityBase::isControlledBySimulator() const -> bool { return true; }
 
+auto EntityBase::setControlledBySimulator(bool /*unused*/) -> void
+{
+  THROW_SEMANTIC_ERROR(
+    getEntityTypename(), " type entities do not support setControlledBySimulator");
+}
+
 auto EntityBase::requestFollowTrajectory(
   const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void
 {

simulation/traffic_simulator/src/entity/entity_manager.cpp

@@ -789,7 +789,7 @@ void EntityManager::requestSpeedChange(
 auto EntityManager::setEntityStatus(
   const std::string & name, const CanonicalizedEntityStatus & status) -> void
 {
-  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && getCurrentTime() > 0 && not isControlledBySimulator(name)) {
     THROW_SEMANTIC_ERROR(
       "You cannot set entity status to the ego vehicle name ", std::quoted(name),
       " after starting scenario.");