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>
@@ -68,6 +69,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
   ServiceWithValidation<tier4_external_api_msgs::srv::Engage>                     requestEngage;
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::InitializeLocalization>      requestInitialPose;
   ServiceWithValidation<autoware_adapi_v1_msgs::srv::SetRoutePoints>              requestSetRoutePoints;
+  ServiceWithValidation<autoware_adapi_v1_msgs::srv::ClearRoute>                  requestClearRoute;
   ServiceWithValidation<tier4_rtc_msgs::srv::AutoModeWithModule>                  requestSetRtcAutoMode;
   ServiceWithValidation<tier4_external_api_msgs::srv::SetVelocityLimit>           requestSetVelocityLimit;
   // clang-format on
@@ -127,6 +129,7 @@ class FieldOperatorApplicationFor<AutowareUniverse>
     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)),
+    requestClearRoute("/api/routing/clear_route", *this),
     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)
@@ -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,15 @@ auto FieldOperatorApplicationFor<AutowareUniverse>::plan(
   });
 }
 
+auto FieldOperatorApplicationFor<AutowareUniverse>::clearRoute() -> void
+{
+  task_queue.delay([this] {
+    auto request = std::make_shared<autoware_adapi_v1_msgs::srv::ClearRoute::Request>();
+
+    requestClearRoute(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

@@ -31,6 +31,16 @@ static auto getParameter(const std::string & name, T value = {})
   return value;
 }
 
+template <typename T>
+static auto getParameterFromExistingNode(
+  rclcpp::Node & node, const std::string & name, T value = {})
+{
+  node.declare_parameter<T>(name, value);
+  node.get_parameter<T>(name, value);
+
+  return value;
+}
+
 EgoEntitySimulation::EgoEntitySimulation(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
@@ -97,22 +107,23 @@ auto EgoEntitySimulation::makeSimulationModel(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters)
   -> const std::shared_ptr<SimModelInterface>
 {
+  rclcpp::Node node{"get_parameter", "simulation"};
   // 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          = getParameterFromExistingNode<double>(node, "acc_time_constant",           0.1);
+  const auto acc_time_delay             = getParameterFromExistingNode<double>(node, "acc_time_delay",              0.1);
+  const auto acceleration_map_path      = getParameterFromExistingNode<std::string>(node, "acceleration_map_path",  "");
+  const auto debug_acc_scaling_factor   = getParameterFromExistingNode<double>(node, "debug_acc_scaling_factor",    1.0);
+  const auto debug_steer_scaling_factor = getParameterFromExistingNode<double>(node, "debug_steer_scaling_factor",  1.0);
+  const auto steer_lim                  = getParameterFromExistingNode<double>(node, "steer_lim",                   parameters.axles.front_axle.max_steering);  // 1.0
+  const auto steer_dead_band            = getParameterFromExistingNode<double>(node, "steer_dead_band",             0.0);
+  const auto steer_rate_lim             = getParameterFromExistingNode<double>(node, "steer_rate_lim",              5.0);
+  const auto steer_time_constant        = getParameterFromExistingNode<double>(node, "steer_time_constant",         0.27);
+  const auto steer_time_delay           = getParameterFromExistingNode<double>(node, "steer_time_delay",            0.24);
+  const auto vel_lim                    = getParameterFromExistingNode<double>(node, "vel_lim",                     parameters.performance.max_speed);  // 50.0
+  const auto vel_rate_lim               = getParameterFromExistingNode<double>(node, "vel_rate_lim",                parameters.performance.max_acceleration);  // 7.0
+  const auto vel_time_constant          = getParameterFromExistingNode<double>(node, "vel_time_constant",           0.1);  /// @note 0.5 is default value on simple_planning_simulator
+  const auto vel_time_delay             = getParameterFromExistingNode<double>(node, "vel_time_delay",              0.1);  /// @note 0.25 is default value on simple_planning_simulator
+  const auto wheel_base                 = getParameterFromExistingNode<double>(node, "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();
 
+  void respawn(
+    const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose,
+    const geometry_msgs::msg::PoseStamped & goal_pose);
+
   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

@@ -120,6 +120,9 @@ class EntityBase
 
   virtual auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter = 0;
 
+  virtual auto getVehicleParameters() const
+    -> const traffic_simulator_msgs::msg::VehicleParameters = 0;
+
   virtual auto getDefaultDynamicConstraints() const
     -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0;
 
@@ -186,6 +189,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,13 +319,15 @@ 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, );
   FORWARD_TO_ENTITY(setLinearVelocity, );
   FORWARD_TO_ENTITY(setMapPose, );
   FORWARD_TO_ENTITY(setTwist, );
   FORWARD_TO_ENTITY(setVelocityLimit, );
+  FORWARD_TO_ENTITY(getVehicleParameters, );
 
 #undef FORWARD_TO_ENTITY
 

simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp

@@ -102,6 +102,11 @@ class MiscObjectEntity : public EntityBase
 
   auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override;
 
+  auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override
+  {
+    THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in MiscObjectEntity");
+  }
+
   void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override;
 
   void setVelocityLimit(double) override{};

simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp

@@ -87,6 +87,10 @@ class PedestrianEntity : public EntityBase
 
   void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &);
 
+  auto getVehicleParameters() const -> const traffic_simulator_msgs::msg::VehicleParameters override
+  {
+    THROW_SEMANTIC_ERROR("getVehicleParameters function cannot be used in PedestrianEntity");
+  }
   void setVelocityLimit(double linear_velocity) override;
 
   void setAccelerationLimit(double acceleration) override;

simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp

@@ -92,6 +92,9 @@ class VehicleEntity : public EntityBase
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
+  auto getVehicleParameters() const
+    -> const traffic_simulator_msgs::msg::VehicleParameters override;
+
   void onUpdate(double current_time, double step_time) override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &);