tier4 · HansRobo · Oct 30, 2023 · Oct 4, 2023 · Oct 4, 2023 · Oct 4, 2023
diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md
@@ -12,9 +12,9 @@ Major Changes :race_car: :red_car: :blue_car:
 
 Bug Fixes:bug:
 
-| Feature | Brief summary | Category | Pull request | Contributor |
-|---------|---------------|----------|--------------|-------------|
-|         |               |          |              |             |
+| Feature                   | Brief summary                                                                   | Category            | Pull request                                                      | Contributor                                 |
+|---------------------------|---------------------------------------------------------------------------------|---------------------|-------------------------------------------------------------------|---------------------------------------------|
+| Deleted entity management | Fix crash caused by `DeleteEntityAction` by handling deleted entity explicitly. | `traffic_simulator` | [#1096](https://github.com/tier4/scenario_simulator_v2/pull/1096) | [f0reachARR](https://github.com/f0reachARR) |
 
 Minor Tweaks :oncoming_police_car:
 
@@ -54,9 +54,9 @@ Major Changes :race_car: :red_car: :blue_car:
 |----------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------|-------------------------------------------------------------------|-----------------------------------------------|
 | OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Added `FaultInjectionAction@v2`, a CustomCommandAction for raising a fault injection event with specified error level.                                    | `openscenario_interpreter`                      | [#1002](https://github.com/tier4/scenario_simulator_v2/pull/1002) | [yamacir-kit](https://github.com/yamacir-kit) |
 | OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Added `V2ITrafficSignalStateAction`, an implementation of `TrafficSignalState` for V2I traffic signals.                                                   | `openscenario_interpreter`, `traffic_simulator` | [#1004](https://github.com/tier4/scenario_simulator_v2/pull/1004) | [HansRobo](https://github.com/HansRobo)       |
-| Add do noting behavior plugin                            | Add do nothing behavior for driving the simulator from Autoware rosbag data.                                                                              | `do_nothing_plugin`,`traffic_simulator`         | [#1001](https://github.com/tier4/scenario_simulator_v2/pull/1011) | [hakuturu583](https://github.com/hakuturu583) |
+| do noting behavior plugin                                | Add do nothing behavior for driving the simulator from Autoware rosbag data.                                                                              | `do_nothing_plugin`,`traffic_simulator`         | [#1001](https://github.com/tier4/scenario_simulator_v2/pull/1011) | [hakuturu583](https://github.com/hakuturu583) |
 | OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Added `RequestToCooperateCommandAction@v1`, a CustomCommandAction to simulate an operation by humans or external applications for a request to cooperate. | `openscenario_interpreter`, `concealer`         | [#1013](https://github.com/tier4/scenario_simulator_v2/pull/1013) | [HansRobo](https://github.com/HansRobo)       |
-|  OpenSCENARIO 1.2 `FollowTrajectoryAction`               | Support `FollowTrajectoryAction` only for vehicle entities directly controlled by the simulator.                                                          | `openscenario_interpreter`, `traffic_simulator` | [#906](https://github.com/tier4/scenario_simulator_v2/pull/906)   | [yamacir-kit](https://github.com/yamacir-kit) |
+| OpenSCENARIO 1.2 `FollowTrajectoryAction`                | Support `FollowTrajectoryAction` only for vehicle entities directly controlled by the simulator.                                                          | `openscenario_interpreter`, `traffic_simulator` | [#906](https://github.com/tier4/scenario_simulator_v2/pull/906)   | [yamacir-kit](https://github.com/yamacir-kit) |
 
 Bug Fixes:bug:
 
@@ -141,12 +141,12 @@ Major Changes :race_car: :red_car: :blue_car:
 
 | Feature                                                           | Brief summary                                                                                               | Category                   | Pull request                                                    | Contributor                                                                        |
 |-------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------|----------------------------|-----------------------------------------------------------------|------------------------------------------------------------------------------------|
-| Add `API::getRelativePose()` argument for function                | Add `API::getRelativePose()` argument for getting relative distance between lanelet pose and entity.        | `traffic_simulator`        | [#809](https://github.com/tier4/scenario_simulator_v2/pull/809) | [hakuturu583](https://github.com/hakuturu583)                                      |
+| `API::getRelativePose()` argument for function                    | Add `API::getRelativePose()` argument for getting relative distance between lanelet pose and entity.        | `traffic_simulator`        | [#809](https://github.com/tier4/scenario_simulator_v2/pull/809) | [hakuturu583](https://github.com/hakuturu583)                                      |
 | RTC (Request to Cooperate)                                        | Experimental support for automatic approval of requests to cooperate from Autoware.Universe.                | `concealer`                | [#818](https://github.com/tier4/scenario_simulator_v2/pull/818) | [yamacir-kit](https://github.com/yamacir-kit)                                      |
 | Experimental UserDefinedValueCondition `RelativeHeadingCondition` | Update `UserDefinedValueCondition` to support new experimental condition `RelativeHeadingCondition`.        | `openscenario_interpreter` | [#830](https://github.com/tier4/scenario_simulator_v2/pull/830) | [yamacir-kit](https://github.com/yamacir-kit)                                      |
 | OpenSCENARIO `ValueConstraint`, `ValueConstraintGroup`            | Add support for `ValueConstraint` and `ValueConstraintGroup`.                                               | `openscenario_interpreter` | [#847](https://github.com/tier4/scenario_simulator_v2/pull/847) | [HansRobo](https://github.com/HansRobo)                                            |
-| Add `API::getTraveledDistance()`                                  | Add `API::getTraveledDistance()` to obtain how far an entity traveled, and remove `TraveledDistanceMetric`. | `traffic_simulator`        | [#858](https://github.com/tier4/scenario_simulator_v2/pull/858) | [hakuturu583](https://github.com/hakuturu583), [shouth](https://github.com/shouth) |
-| Improve ego lane matching logic                                   | Retry matching to lanelet without using route information from Autoware.                                    | `traffic_simulator`        | [#864](https://github.com/tier4/scenario_simulator_v2/pull/864) | [hakuturu583](https://github.com/hakuturu583)                                      |
+| `API::getTraveledDistance()`                                      | Add `API::getTraveledDistance()` to obtain how far an entity traveled, and remove `TraveledDistanceMetric`. | `traffic_simulator`        | [#858](https://github.com/tier4/scenario_simulator_v2/pull/858) | [hakuturu583](https://github.com/hakuturu583), [shouth](https://github.com/shouth) |
+| Ego lane matching logic improvement                               | Retry matching to lanelet without using route information from Autoware.                                    | `traffic_simulator`        | [#864](https://github.com/tier4/scenario_simulator_v2/pull/864) | [hakuturu583](https://github.com/hakuturu583)                                      |
 
 Bug Fixes:bug:
 

diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt
@@ -37,6 +37,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/data_type/lane_change.cpp
   src/data_type/lanelet_pose.cpp
   src/data_type/speed_change.cpp
+  src/entity/deleted_entity.cpp
   src/entity/ego_entity.cpp
   src/entity/entity_base.cpp
   src/entity/entity_manager.cpp

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/deleted_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/deleted_entity.hpp
@@ -0,0 +1,107 @@
+// 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.
+
+#ifndef TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_
+#define TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_
+
+#include <optional>
+#include <traffic_simulator/entity/entity_base.hpp>
+#include <traffic_simulator_msgs/msg/misc_object_parameters.hpp>
+
+namespace traffic_simulator
+{
+namespace entity
+{
+class DeletedEntity : public EntityBase
+{
+public:
+  // Dummy ID for internal use
+  static constexpr uint8_t ENTITY_TYPE_ID = 0xFF;
+
+  explicit DeletedEntity(
+    const std::string & name, const CanonicalizedEntityStatus &,
+    const std::shared_ptr<hdmap_utils::HdMapUtils> &);
+
+  void onUpdate(double, double) override {}
+
+  auto getCurrentAction() const -> std::string override;
+
+  auto getDefaultDynamicConstraints() const
+    -> const traffic_simulator_msgs::msg::DynamicConstraints & override;
+
+  auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override
+  {
+    static traffic_simulator_msgs::msg::EntityType type;
+    type.type = ENTITY_TYPE_ID;  // Dummy ID for internal use
+    return type;
+  }
+
+  auto getEntityTypename() const -> const std::string & override
+  {
+    static const std::string result = "DeletedEntity";
+    return result;
+  }
+
+  ~DeletedEntity() override = default;
+
+  auto getGoalPoses() -> std::vector<CanonicalizedLaneletPose> override { return {}; }
+
+  std::optional<traffic_simulator_msgs::msg::Obstacle> getObstacle() override
+  {
+    return std::nullopt;
+  }
+
+  auto getRouteLanelets(double) -> lanelet::Ids override { return {}; }
+
+  auto fillLaneletPose(CanonicalizedEntityStatus &) -> void override;
+
+  auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override
+  {
+    return traffic_simulator_msgs::msg::WaypointsArray();
+  }
+
+  void requestSpeedChange(double, bool) override {}
+
+  void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool) override {}
+
+  void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override {}
+
+  void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override {}
+
+  void requestAcquirePosition(const CanonicalizedLaneletPose &) override {}
+
+  void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {}
+
+  void requestSpeedChange(
+    const double, const speed_change::Transition, const speed_change::Constraint,
+    const bool) override
+  {
+  }
+
+  auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override;
+
+  void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override {}
+
+  void setAccelerationLimit(double) override {}
+
+  void setAccelerationRateLimit(double) override {}
+
+  void setDecelerationLimit(double) override {}
+
+  void setDecelerationRateLimit(double) override {}
+};
+}  // namespace entity
+}  // namespace traffic_simulator
+
+#endif  // TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -32,6 +32,7 @@
 #include <traffic_simulator/api/configuration.hpp>
 #include <traffic_simulator/data_type/lane_change.hpp>
 #include <traffic_simulator/data_type/speed_change.hpp>
+#include <traffic_simulator/entity/deleted_entity.hpp>
 #include <traffic_simulator/entity/ego_entity.hpp>
 #include <traffic_simulator/entity/entity_base.hpp>
 #include <traffic_simulator/entity/misc_object_entity.hpp>

diff --git a/simulation/traffic_simulator/src/entity/deleted_entity.cpp b/simulation/traffic_simulator/src/entity/deleted_entity.cpp
@@ -0,0 +1,69 @@
+// 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 <limits>
+#include <traffic_simulator/entity/deleted_entity.hpp>
+
+namespace traffic_simulator
+{
+namespace entity
+{
+DeletedEntity::DeletedEntity(
+  const std::string & name, const CanonicalizedEntityStatus & entity_status,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+: EntityBase(name, entity_status, hdmap_utils_ptr)
+{
+  auto status = static_cast<EntityStatus>(status_);
+  status.action_status.twist = geometry_msgs::msg::Twist();
+  status.action_status.accel = geometry_msgs::msg::Accel();
+  status.action_status.linear_jerk = 0;
+  status.action_status.current_action = "static";
+  status.bounding_box.dimensions.x = 0;
+  status.bounding_box.dimensions.y = 0;
+  status.bounding_box.dimensions.z = 0;
+  status.pose.position.x = std::numeric_limits<double>::infinity();
+  status.pose.position.y = std::numeric_limits<double>::infinity();
+  status.pose.position.z = std::numeric_limits<double>::infinity();
+  status.lanelet_pose_valid = false;
+  status_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_);
+  status_before_update_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_);
+}
+
+auto DeletedEntity::getCurrentAction() const -> std::string { return "static"; }
+
+auto DeletedEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter
+{
+  return traffic_simulator_msgs::msg::BehaviorParameter();
+}
+
+auto DeletedEntity::getDefaultDynamicConstraints() const
+  -> const traffic_simulator_msgs::msg::DynamicConstraints &
+{
+  static const auto default_dynamic_constraints = []() {
+    auto dynamic_constraints = traffic_simulator_msgs::msg::DynamicConstraints();
+    dynamic_constraints.max_speed = 0.0;
+    dynamic_constraints.max_acceleration = 0.0;
+    dynamic_constraints.max_acceleration_rate = 0.0;
+    dynamic_constraints.max_deceleration = 0.0;
+    dynamic_constraints.max_deceleration_rate = 0.0;
+    return dynamic_constraints;
+  }();
+
+  return default_dynamic_constraints;
+}
+
+auto DeletedEntity::fillLaneletPose(CanonicalizedEntityStatus &) -> void {}
+
+}  // namespace entity
+}  // namespace traffic_simulator
diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp
@@ -86,7 +86,13 @@ visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const
 
 bool EntityManager::despawnEntity(const std::string & name)
 {
-  return entityExists(name) && entities_.erase(name);
+  if (!entityExists(name)) {
+    return false;
+  }
+
+  entities_[name].reset(new DeletedEntity(name, getEntityStatus(name), hdmap_utils_ptr_));
+
+  return true;
 }
 
 bool EntityManager::entityExists(const std::string & name)
@@ -135,7 +141,11 @@ auto EntityManager::getEntityNames() const -> const std::vector<std::string>
 {
   std::vector<std::string> names{};
   for (const auto & each : entities_) {
-    names.push_back(each.first);
+    // Add filter for DeletedEntity because this list is used on SimpleSensorSimulator which do not
+    // know DeletedEntity.
+    if (each.second->getEntityType().type != DeletedEntity::ENTITY_TYPE_ID) {
+      names.push_back(each.first);
+    }
   }
   return names;
 }