From eecec3cdc544f7f295a20087e516f0ac1c83b165 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Tue, 28 May 2024 15:14:08 +0200
Subject: [PATCH 01/12] Remove step_time_ and current_time_ data members from
 EntityManager

Adjust the code so that the time is managed in API only

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/api/api.hpp     | 17 +++----
 .../entity/entity_manager.hpp                 | 35 +++++++-------
 .../src/entity/entity_manager.cpp             | 47 +++++++++----------
 3 files changed, 48 insertions(+), 51 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
index 249d182ae4f..61d7a4ee26d 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -136,10 +136,10 @@ class API
       if (behavior == VehicleBehavior::autoware()) {
         return entity_manager_ptr_->entityExists(name) or
                entity_manager_ptr_->spawnEntity<entity::EgoEntity>(
-                 name, pose, parameters, configuration);
+                 name, pose, parameters, getCurrentTime(), configuration);
       } else {
         return entity_manager_ptr_->spawnEntity<entity::VehicleEntity>(
-          name, pose, parameters, behavior);
+          name, pose, parameters, getCurrentTime(), behavior);
       }
     };
 
@@ -174,7 +174,7 @@ class API
   {
     auto register_to_entity_manager = [&]() {
       return entity_manager_ptr_->spawnEntity<entity::PedestrianEntity>(
-        name, pose, parameters, behavior);
+        name, pose, parameters, getCurrentTime(), behavior);
     };
 
     auto register_to_environment_simulator = [&]() {
@@ -203,7 +203,8 @@ class API
     const std::string & model3d = "")
   {
     auto register_to_entity_manager = [&]() {
-      return entity_manager_ptr_->spawnEntity<entity::MiscObjectEntity>(name, pose, parameters);
+      return entity_manager_ptr_->spawnEntity<entity::MiscObjectEntity>(
+        name, pose, parameters, getCurrentTime());
     };
 
     auto register_to_environment_simulator = [&]() {
@@ -297,7 +298,7 @@ class API
     const lane_change::Constraint & constraint);
 
   // clang-format off
-#define FORWARD_TO_ENTITY_MANAGER(NAME)                                    \
+#define FORWARD_TO_ENTITY_MANAGER(NAME, ...)                               \
   /*!                                                                      \
    @brief Forward to arguments to the EntityManager::NAME function.        \
    @return return value of the EntityManager::NAME function.               \
@@ -307,7 +308,7 @@ class API
   decltype(auto) NAME(Ts &&... xs)                                         \
   {                                                                        \
     assert(entity_manager_ptr_);                                           \
-    return (*entity_manager_ptr_).NAME(std::forward<decltype(xs)>(xs)...); \
+    return (*entity_manager_ptr_).NAME(__VA_ARGS__ std::forward<decltype(xs)>(xs)...); \
   }                                                                        \
   static_assert(true, "")
   // clang-format on
@@ -343,9 +344,9 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition);
   FORWARD_TO_ENTITY_MANAGER(requestAssignRoute);
   FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory);
-  FORWARD_TO_ENTITY_MANAGER(requestSpeedChange);
+  FORWARD_TO_ENTITY_MANAGER(requestSpeedChange, getCurrentTime(), );
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
-  FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin);
+  FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin, getCurrentTime(), );
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(setAcceleration);
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
index f758d43319f..d81d02df15f 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -94,10 +94,6 @@ class EntityManager
 
   std::unordered_map<std::string, std::shared_ptr<traffic_simulator::entity::EntityBase>> entities_;
 
-  double step_time_;
-
-  double current_time_;
-
   bool npc_logic_started_;
 
   using EntityStatusWithTrajectoryArray =
@@ -163,7 +159,6 @@ class EntityManager
     broadcaster_(node),
     base_link_broadcaster_(node),
     clock_ptr_(node->get_clock()),
-    current_time_(std::numeric_limits<double>::quiet_NaN()),
     npc_logic_started_(false),
     entity_status_array_pub_ptr_(rclcpp::create_publisher<EntityStatusWithTrajectoryArray>(
       node, "entity/status", EntityMarkerQoS(),
@@ -304,22 +299,26 @@ class EntityManager
 
   bool trafficLightsChanged();
 
-  void requestSpeedChange(const std::string & name, double target_speed, bool continuous);
+  void requestSpeedChange(
+    const double current_time, const std::string & name, double target_speed, bool continuous);
 
   void requestSpeedChange(
-    const std::string & name, const double target_speed, const speed_change::Transition transition,
-    const speed_change::Constraint constraint, const bool continuous);
+    const double current_time, const std::string & name, const double target_speed,
+    const speed_change::Transition transition, const speed_change::Constraint constraint,
+    const bool continuous);
 
   void requestSpeedChange(
-    const std::string & name, const speed_change::RelativeTargetSpeed & target_speed,
-    bool continuous);
+    const double current_time, const std::string & name,
+    const speed_change::RelativeTargetSpeed & target_speed, bool continuous);
 
   void requestSpeedChange(
-    const std::string & name, const speed_change::RelativeTargetSpeed & target_speed,
+    const double current_time, const std::string & name,
+    const speed_change::RelativeTargetSpeed & target_speed,
     const speed_change::Transition transition, const speed_change::Constraint constraint,
     const bool continuous);
 
-  auto updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus &;
+  auto updateNpcLogic(const std::string & name, const double current_time, const double step_time)
+    -> const CanonicalizedEntityStatus &;
 
   void broadcastEntityTransform();
 
@@ -333,8 +332,6 @@ class EntityManager
 
   bool entityExists(const std::string & name);
 
-  auto getCurrentTime() const noexcept -> double;
-
   auto getEntityNames() const -> const std::vector<std::string>;
 
   auto getEntity(const std::string & name) const
@@ -352,8 +349,6 @@ class EntityManager
   auto getPedestrianParameters(const std::string & name) const
     -> const traffic_simulator_msgs::msg::PedestrianParameters &;
 
-  auto getStepTime() const noexcept -> double;
-
   auto getVehicleParameters(const std::string & name) const
     -> const traffic_simulator_msgs::msg::VehicleParameters &;
 
@@ -415,13 +410,15 @@ class EntityManager
    * @sa traffic_simulator::entity::PedestrianEntity::BuiltinBehavior
    * @sa traffic_simulator::entity::VehicleEntity::BuiltinBehavior
    */
-  void resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name);
+  void resetBehaviorPlugin(
+    const double current_time, const std::string & name, const std::string & behavior_plugin_name);
 
   void setVerbose(const bool verbose);
 
   template <typename Entity, typename Pose, typename Parameters, typename... Ts>
   auto spawnEntity(
-    const std::string & name, const Pose & pose, const Parameters & parameters, Ts &&... xs)
+    const std::string & name, const Pose & pose, const Parameters & parameters,
+    const double current_time, Ts &&... xs)
   {
     auto makeEntityStatus = [&]() {
       EntityStatus entity_status;
@@ -444,7 +441,7 @@ class EntityManager
       }
 
       entity_status.subtype = parameters.subtype;
-      entity_status.time = getCurrentTime();
+      entity_status.time = current_time;
       entity_status.name = name;
       entity_status.bounding_box = parameters.bounding_box;
       entity_status.action_status = traffic_simulator_msgs::msg::ActionStatus();
diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp
index 9e9412f26fd..fc47f42bb36 100644
--- a/simulation/traffic_simulator/src/entity/entity_manager.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp
@@ -131,8 +131,6 @@ bool EntityManager::entityExists(const std::string & name)
   return entities_.find(name) != std::end(entities_);
 }
 
-auto EntityManager::getCurrentTime() const noexcept -> double { return current_time_; }
-
 auto EntityManager::getEntityNames() const -> const std::vector<std::string>
 {
   std::vector<std::string> names{};
@@ -214,8 +212,6 @@ auto EntityManager::getPedestrianParameters(const std::string & name) const
     "Please check description of the scenario and entity type of the Entity: " + name);
 }
 
-auto EntityManager::getStepTime() const noexcept -> double { return step_time_; }
-
 auto EntityManager::getVehicleParameters(const std::string & name) const
   -> const traffic_simulator_msgs::msg::VehicleParameters &
 {
@@ -304,7 +300,7 @@ void EntityManager::requestLaneChange(
 }
 
 void EntityManager::resetBehaviorPlugin(
-  const std::string & name, const std::string & behavior_plugin_name)
+  const double current_time, const std::string & name, const std::string & behavior_plugin_name)
 {
   const auto & status = getEntityStatus(name);
   const auto behavior_parameter = getBehaviorParameter(name);
@@ -318,11 +314,13 @@ void EntityManager::resetBehaviorPlugin(
   } else if (is<VehicleEntity>(name)) {
     const auto parameters = getVehicleParameters(name);
     despawnEntity(name);
-    spawnEntity<VehicleEntity>(name, status.getMapPose(), parameters, behavior_plugin_name);
+    spawnEntity<VehicleEntity>(
+      name, status.getMapPose(), parameters, current_time, behavior_plugin_name);
   } else if (is<PedestrianEntity>(name)) {
     const auto parameters = getPedestrianParameters(name);
     despawnEntity(name);
-    spawnEntity<PedestrianEntity>(name, status.getMapPose(), parameters, behavior_plugin_name);
+    spawnEntity<PedestrianEntity>(
+      name, status.getMapPose(), parameters, current_time, behavior_plugin_name);
   } else {
     THROW_SIMULATION_ERROR(
       "Entity :", name, "is unkown entity type.", "Please contact to developer.");
@@ -340,39 +338,41 @@ bool EntityManager::trafficLightsChanged()
 }
 
 void EntityManager::requestSpeedChange(
-  const std::string & name, double target_speed, bool continuous)
+  const double current_time, const std::string & name, double target_speed, bool continuous)
 {
-  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && current_time > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, continuous);
 }
 
 void EntityManager::requestSpeedChange(
-  const std::string & name, const double target_speed, const speed_change::Transition transition,
-  const speed_change::Constraint constraint, const bool continuous)
+  const double current_time, const std::string & name, const double target_speed,
+  const speed_change::Transition transition, const speed_change::Constraint constraint,
+  const bool continuous)
 {
-  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && current_time > 0.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);
 }
 
 void EntityManager::requestSpeedChange(
-  const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
+  const double current_time, const std::string & name,
+  const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
 {
-  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && current_time > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   return entities_.at(name)->requestSpeedChange(target_speed, continuous);
 }
 
 void EntityManager::requestSpeedChange(
-  const std::string & name, const speed_change::RelativeTargetSpeed & target_speed,
-  const speed_change::Transition transition, const speed_change::Constraint constraint,
-  const bool continuous)
+  const double current_time, const std::string & name,
+  const speed_change::RelativeTargetSpeed & target_speed, const speed_change::Transition transition,
+  const speed_change::Constraint constraint, const bool continuous)
 {
-  if (is<EgoEntity>(name) && getCurrentTime() > 0) {
+  if (is<EgoEntity>(name) && current_time > 0.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);
@@ -386,12 +386,14 @@ void EntityManager::setVerbose(const bool verbose)
   }
 }
 
-auto EntityManager::updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus &
+auto EntityManager::updateNpcLogic(
+  const std::string & name, const double current_time, const double step_time)
+  -> const CanonicalizedEntityStatus &
 {
   if (configuration.verbose) {
     std::cout << "update " << name << " behavior" << std::endl;
   }
-  entities_[name]->onUpdate(current_time_, step_time_);
+  entities_[name]->onUpdate(current_time, step_time);
   return entities_[name]->getStatus();
 }
 
@@ -399,8 +401,6 @@ void EntityManager::update(const double current_time, const double step_time)
 {
   traffic_simulator::helper::StopWatch<std::chrono::milliseconds> stop_watch_update(
     "EntityManager::update", configuration.verbose);
-  step_time_ = step_time;
-  current_time_ = current_time;
   setVerbose(configuration.verbose);
   if (npc_logic_started_) {
     conventional_traffic_light_updater_.createTimer(
@@ -416,7 +416,7 @@ void EntityManager::update(const double current_time, const double step_time)
   }
   all_status.clear();
   for (auto && [name, entity] : entities_) {
-    all_status.emplace(name, updateNpcLogic(name));
+    all_status.emplace(name, updateNpcLogic(name, current_time, step_time));
   }
   for (auto && [name, entity] : entities_) {
     entity->setOtherStatus(all_status);
@@ -444,7 +444,6 @@ void EntityManager::update(const double current_time, const double step_time)
   if (configuration.verbose) {
     stop_watch_update.print();
   }
-  current_time_ += step_time;
 }
 
 void EntityManager::updateHdmapMarker()

From fababb45457f8cb82652871e6c92fab51e5af02e Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Tue, 28 May 2024 15:18:42 +0200
Subject: [PATCH 02/12] Correct style

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/api/api.hpp     | 22 +++++++++----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
index 61d7a4ee26d..7cbe497c3d2 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -298,18 +298,18 @@ class API
     const lane_change::Constraint & constraint);
 
   // clang-format off
-#define FORWARD_TO_ENTITY_MANAGER(NAME, ...)                               \
-  /*!                                                                      \
-   @brief Forward to arguments to the EntityManager::NAME function.        \
-   @return return value of the EntityManager::NAME function.               \
-   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.     \
-   */                                                                      \
-  template <typename... Ts>                                                \
-  decltype(auto) NAME(Ts &&... xs)                                         \
-  {                                                                        \
-    assert(entity_manager_ptr_);                                           \
+#define FORWARD_TO_ENTITY_MANAGER(NAME, ...)                                           \
+  /*!                                                                                  \
+   @brief Forward to arguments to the EntityManager::NAME function.                    \
+   @return return value of the EntityManager::NAME function.                           \
+   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.                 \
+   */                                                                                  \
+  template <typename... Ts>                                                            \
+  decltype(auto) NAME(Ts &&... xs)                                                     \
+  {                                                                                    \
+    assert(entity_manager_ptr_);                                                       \
     return (*entity_manager_ptr_).NAME(__VA_ARGS__ std::forward<decltype(xs)>(xs)...); \
-  }                                                                        \
+  }                                                                                    \
   static_assert(true, "")
   // clang-format on
 

From 23e5625a901a633a052be8aaa2cb1cc7ff3a288b Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Wed, 29 May 2024 12:43:51 +0200
Subject: [PATCH 03/12] Move requestSpeedChange time check responsibility to
 EgoEntity and simplify

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/api/api.hpp     | 28 +++++------
 .../entity/entity_manager.hpp                 | 22 +--------
 .../src/entity/ego_entity.cpp                 |  6 +++
 .../src/entity/entity_manager.cpp             | 47 ++-----------------
 4 files changed, 25 insertions(+), 78 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
index 7cbe497c3d2..5f77bfbe897 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -298,18 +298,18 @@ class API
     const lane_change::Constraint & constraint);
 
   // clang-format off
-#define FORWARD_TO_ENTITY_MANAGER(NAME, ...)                                           \
-  /*!                                                                                  \
-   @brief Forward to arguments to the EntityManager::NAME function.                    \
-   @return return value of the EntityManager::NAME function.                           \
-   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.                 \
-   */                                                                                  \
-  template <typename... Ts>                                                            \
-  decltype(auto) NAME(Ts &&... xs)                                                     \
-  {                                                                                    \
-    assert(entity_manager_ptr_);                                                       \
-    return (*entity_manager_ptr_).NAME(__VA_ARGS__ std::forward<decltype(xs)>(xs)...); \
-  }                                                                                    \
+#define FORWARD_TO_ENTITY_MANAGER(NAME)                                  \
+  /*!                                                                    \
+   @brief Forward to arguments to the EntityManager::NAME function.      \
+   @return return value of the EntityManager::NAME function.             \
+   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.   \
+   */                                                                    \
+  template <typename... Ts>                                              \
+  decltype(auto) NAME(Ts &&... xs)                                       \
+  {                                                                      \
+    assert(entity_manager_ptr_);                                         \
+    return entity_manager_ptr_->NAME(std::forward<decltype(xs)>(xs)...); \
+  }                                                                      \
   static_assert(true, "")
   // clang-format on
 
@@ -344,9 +344,9 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestAcquirePosition);
   FORWARD_TO_ENTITY_MANAGER(requestAssignRoute);
   FORWARD_TO_ENTITY_MANAGER(requestFollowTrajectory);
-  FORWARD_TO_ENTITY_MANAGER(requestSpeedChange, getCurrentTime(), );
+  FORWARD_TO_ENTITY_MANAGER(requestSpeedChange);
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
-  FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin, getCurrentTime(), );
+  FORWARD_TO_ENTITY_MANAGER(resetBehaviorPlugin);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(setAcceleration);
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
index d81d02df15f..128f1510919 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -292,6 +292,7 @@ class EntityManager
   FORWARD_TO_ENTITY(setMapPose, );
   FORWARD_TO_ENTITY(setTwist, );
   FORWARD_TO_ENTITY(setVelocityLimit, );
+  FORWARD_TO_ENTITY(requestSpeedChange, );
 
 #undef FORWARD_TO_ENTITY
 
@@ -299,24 +300,6 @@ class EntityManager
 
   bool trafficLightsChanged();
 
-  void requestSpeedChange(
-    const double current_time, const std::string & name, double target_speed, bool continuous);
-
-  void requestSpeedChange(
-    const double current_time, const std::string & name, const double target_speed,
-    const speed_change::Transition transition, const speed_change::Constraint constraint,
-    const bool continuous);
-
-  void requestSpeedChange(
-    const double current_time, const std::string & name,
-    const speed_change::RelativeTargetSpeed & target_speed, bool continuous);
-
-  void requestSpeedChange(
-    const double current_time, const std::string & name,
-    const speed_change::RelativeTargetSpeed & target_speed,
-    const speed_change::Transition transition, const speed_change::Constraint constraint,
-    const bool continuous);
-
   auto updateNpcLogic(const std::string & name, const double current_time, const double step_time)
     -> const CanonicalizedEntityStatus &;
 
@@ -410,8 +393,7 @@ class EntityManager
    * @sa traffic_simulator::entity::PedestrianEntity::BuiltinBehavior
    * @sa traffic_simulator::entity::VehicleEntity::BuiltinBehavior
    */
-  void resetBehaviorPlugin(
-    const double current_time, const std::string & name, const std::string & behavior_plugin_name);
+  void resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name);
 
   void setVerbose(const bool verbose);
 
diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index 80065717745..81ee95df569 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -276,6 +276,9 @@ auto EgoEntity::setBehaviorParameter(
 
 void EgoEntity::requestSpeedChange(double value, bool)
 {
+  if (getStatus().getTime() > 0.0) {
+    THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
+  }
   target_speed_ = value;
   field_operator_application->restrictTargetSpeed(value);
 }
@@ -283,6 +286,9 @@ void EgoEntity::requestSpeedChange(double value, bool)
 void EgoEntity::requestSpeedChange(
   const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/)
 {
+  THROW_SEMANTIC_ERROR(
+    "The traffic_simulator's request to set speed to the Ego type entity is for initialization "
+    "purposes only.");
 }
 
 auto EgoEntity::setVelocityLimit(double value) -> void  //
diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp
index fc47f42bb36..cd75401f850 100644
--- a/simulation/traffic_simulator/src/entity/entity_manager.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp
@@ -300,7 +300,7 @@ void EntityManager::requestLaneChange(
 }
 
 void EntityManager::resetBehaviorPlugin(
-  const double current_time, const std::string & name, const std::string & behavior_plugin_name)
+  const std::string & name, const std::string & behavior_plugin_name)
 {
   const auto & status = getEntityStatus(name);
   const auto behavior_parameter = getBehaviorParameter(name);
@@ -315,12 +315,12 @@ void EntityManager::resetBehaviorPlugin(
     const auto parameters = getVehicleParameters(name);
     despawnEntity(name);
     spawnEntity<VehicleEntity>(
-      name, status.getMapPose(), parameters, current_time, behavior_plugin_name);
+      name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name);
   } else if (is<PedestrianEntity>(name)) {
     const auto parameters = getPedestrianParameters(name);
     despawnEntity(name);
     spawnEntity<PedestrianEntity>(
-      name, status.getMapPose(), parameters, current_time, behavior_plugin_name);
+      name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name);
   } else {
     THROW_SIMULATION_ERROR(
       "Entity :", name, "is unkown entity type.", "Please contact to developer.");
@@ -337,47 +337,6 @@ bool EntityManager::trafficLightsChanged()
          v2i_traffic_light_manager_ptr_->hasAnyLightChanged();
 }
 
-void EntityManager::requestSpeedChange(
-  const double current_time, const std::string & name, double target_speed, bool continuous)
-{
-  if (is<EgoEntity>(name) && current_time > 0.0) {
-    THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
-  }
-  return entities_.at(name)->requestSpeedChange(target_speed, continuous);
-}
-
-void EntityManager::requestSpeedChange(
-  const double current_time, const std::string & name, const double target_speed,
-  const speed_change::Transition transition, const speed_change::Constraint constraint,
-  const bool continuous)
-{
-  if (is<EgoEntity>(name) && current_time > 0.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);
-}
-
-void EntityManager::requestSpeedChange(
-  const double current_time, const std::string & name,
-  const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
-{
-  if (is<EgoEntity>(name) && current_time > 0.0) {
-    THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
-  }
-  return entities_.at(name)->requestSpeedChange(target_speed, continuous);
-}
-
-void EntityManager::requestSpeedChange(
-  const double current_time, const std::string & name,
-  const speed_change::RelativeTargetSpeed & target_speed, const speed_change::Transition transition,
-  const speed_change::Constraint constraint, const bool continuous)
-{
-  if (is<EgoEntity>(name) && current_time > 0.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);
-}
-
 void EntityManager::setVerbose(const bool verbose)
 {
   configuration.verbose = verbose;

From dc9f6fa29b742a2c42b29a5fbfb2f89b4995ff07 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Mon, 3 Jun 2024 11:37:30 +0200
Subject: [PATCH 04/12] Revert to previous macro definition

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/api/api.hpp     | 24 +++++++++----------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
index 5f77bfbe897..34d2e6a65d5 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -298,18 +298,18 @@ class API
     const lane_change::Constraint & constraint);
 
   // clang-format off
-#define FORWARD_TO_ENTITY_MANAGER(NAME)                                  \
-  /*!                                                                    \
-   @brief Forward to arguments to the EntityManager::NAME function.      \
-   @return return value of the EntityManager::NAME function.             \
-   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.   \
-   */                                                                    \
-  template <typename... Ts>                                              \
-  decltype(auto) NAME(Ts &&... xs)                                       \
-  {                                                                      \
-    assert(entity_manager_ptr_);                                         \
-    return entity_manager_ptr_->NAME(std::forward<decltype(xs)>(xs)...); \
-  }                                                                      \
+#define FORWARD_TO_ENTITY_MANAGER(NAME)                                    \
+  /*!                                                                      \
+   @brief Forward to arguments to the EntityManager::NAME function.        \
+   @return return value of the EntityManager::NAME function.               \
+   @note This function was defined by FORWARD_TO_ENTITY_MANAGER macro.     \
+   */                                                                      \
+  template <typename... Ts>                                                \
+  decltype(auto) NAME(Ts &&... xs)                                         \
+  {                                                                        \
+    assert(entity_manager_ptr_);                                           \
+    return (*entity_manager_ptr_).NAME(std::forward<decltype(xs)>(xs)...); \
+  }                                                                        \
   static_assert(true, "")
   // clang-format on
 

From f6351837b6f211339c3b5ec91d17a512eb5e8def Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Mon, 3 Jun 2024 11:58:49 +0200
Subject: [PATCH 05/12] Use member instead of getter

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index 81ee95df569..62b1620e0a1 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -276,7 +276,7 @@ auto EgoEntity::setBehaviorParameter(
 
 void EgoEntity::requestSpeedChange(double value, bool)
 {
-  if (getStatus().getTime() > 0.0) {
+  if (status_.getTime() > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   }
   target_speed_ = value;

From 0c81009a20fcae51d461f9011efcc6a12e3d6910 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Mon, 3 Jun 2024 12:29:02 +0200
Subject: [PATCH 06/12] Add const to time argument

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/entity/ego_entity.hpp           | 2 +-
 .../include/traffic_simulator/entity/entity_base.hpp          | 4 ++--
 .../include/traffic_simulator/entity/pedestrian_entity.hpp    | 2 +-
 .../include/traffic_simulator/entity/vehicle_entity.hpp       | 2 +-
 simulation/traffic_simulator/src/entity/ego_entity.cpp        | 2 +-
 simulation/traffic_simulator/src/entity/entity_base.cpp       | 4 ++--
 .../traffic_simulator/src/entity/misc_object_entity.cpp       | 2 +-
 simulation/traffic_simulator/src/entity/pedestrian_entity.cpp | 2 +-
 simulation/traffic_simulator/src/entity/vehicle_entity.cpp    | 2 +-
 9 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
index b8741f2d11b..b38389b094c 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
@@ -85,7 +85,7 @@ class EgoEntity : public VehicleEntity
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(double current_time, double step_time) override;
+  void onUpdate(const double current_time, const double step_time) override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &) override;
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
index ba61c7e1342..26a7aa1539a 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -124,9 +124,9 @@ class EntityBase
 
   virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
 
-  virtual void onUpdate(double current_time, double step_time);
+  virtual void onUpdate(const double current_time, const double step_time);
 
-  virtual void onPostUpdate(double current_time, double step_time);
+  virtual void onPostUpdate(const double current_time, const double step_time);
 
   /*   */ void resetDynamicConstraints();
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
index 6b81143d1db..cc46cb3374d 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
@@ -70,7 +70,7 @@ class PedestrianEntity : public EntityBase
 
   auto getEntityTypename() const -> const std::string & override;
 
-  void onUpdate(double current_time, double step_time) override;
+  void onUpdate(const double current_time, const double step_time) override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) override;
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
index 6949598cbc6..04ec4577637 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
@@ -92,7 +92,7 @@ class VehicleEntity : public EntityBase
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(double current_time, double step_time) override;
+  void onUpdate(const double current_time, const double step_time) override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &);
 
diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index 62b1620e0a1..ccc5ad7b5db 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -148,7 +148,7 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr
   return field_operator_application->getWaypoints();
 }
 
-void EgoEntity::onUpdate(double current_time, double step_time)
+void EgoEntity::onUpdate(const double current_time, const double step_time)
 {
   EntityBase::onUpdate(current_time, step_time);
 
diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
index 2287e1f6c82..9d37de608fd 100644
--- a/simulation/traffic_simulator/src/entity/entity_base.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -99,7 +99,7 @@ auto EntityBase::isTargetSpeedReached(const speed_change::RelativeTargetSpeed &
   return isTargetSpeedReached(target_speed.getAbsoluteValue(getStatus(), other_status_));
 }
 
-void EntityBase::onUpdate(double /*current_time*/, double step_time)
+void EntityBase::onUpdate(const double /*current_time*/, const double step_time)
 {
   job_list_.update(step_time, job::Event::PRE_UPDATE);
   status_before_update_.set(status_);
@@ -108,7 +108,7 @@ void EntityBase::onUpdate(double /*current_time*/, double step_time)
       step_time, name);
 }
 
-void EntityBase::onPostUpdate(double /*current_time*/, double step_time)
+void EntityBase::onPostUpdate(const double /*current_time*/, const double step_time)
 {
   job_list_.update(step_time, job::Event::POST_UPDATE);
 }
diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
index c8e650ea6e2..cc60bc2ac90 100644
--- a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
@@ -26,7 +26,7 @@ MiscObjectEntity::MiscObjectEntity(
 {
 }
 
-void MiscObjectEntity::onUpdate(double, double step_time)
+void MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time)
 {
   setTwist(geometry_msgs::msg::Twist());
   setAcceleration(geometry_msgs::msg::Accel());
diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
index c05685517f4..5f8572d6203 100644
--- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
@@ -248,7 +248,7 @@ void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate)
   setBehaviorParameter(behavior_parameter);
 }
 
-void PedestrianEntity::onUpdate(double current_time, double step_time)
+void PedestrianEntity::onUpdate(const double current_time, const double step_time)
 {
   EntityBase::onUpdate(current_time, step_time);
   if (npc_logic_started_) {
diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
index 1370c6b0ef4..f6278f79000 100644
--- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -139,7 +139,7 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin
   }
 }
 
-void VehicleEntity::onUpdate(double current_time, double step_time)
+void VehicleEntity::onUpdate(const double current_time, const double step_time)
 {
   EntityBase::onUpdate(current_time, step_time);
   if (npc_logic_started_) {

From d2420221da97720a8237c0a8066b515bca76b9d5 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Mon, 3 Jun 2024 13:47:57 +0200
Subject: [PATCH 07/12] Add const to time argument in behavior

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/behavior_tree_plugin/pedestrian/behavior_tree.hpp | 4 ++--
 .../include/behavior_tree_plugin/vehicle/behavior_tree.hpp    | 4 ++--
 .../behavior_tree_plugin/src/pedestrian/behavior_tree.cpp     | 4 ++--
 simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp | 4 ++--
 .../traffic_simulator/behavior/behavior_plugin_base.hpp       | 2 +-
 .../behavior/longitudinal_speed_planning.hpp                  | 2 +-
 .../traffic_simulator/src/behavior/follow_trajectory.cpp      | 2 +-
 .../src/behavior/longitudinal_speed_planning.cpp              | 3 ++-
 8 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
index bd651428aba..d82d5375c25 100644
--- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
+++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
@@ -39,7 +39,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 {
 public:
   void configure(const rclcpp::Logger & logger) override;
-  void update(double current_time, double step_time) override;
+  void update(const double current_time, const double step_time) override;
   const std::string & getCurrentAction() const override;
 
 #define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
@@ -76,7 +76,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(double current_time, double step_time);
+  BT::NodeStatus tickOnce(const double current_time, const double step_time);
   auto createBehaviorTree(const std::string & format_path) -> BT::Tree;
   BT::BehaviorTreeFactory factory_;
   BT::Tree tree_;
diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
index 885da5cd5a5..219a3b27743 100644
--- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
+++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
@@ -38,7 +38,7 @@ namespace entity_behavior
 class VehicleBehaviorTree : public BehaviorPluginBase
 {
 public:
-  void update(double current_time, double step_time) override;
+  void update(const double current_time, const double step_time) override;
   void configure(const rclcpp::Logger & logger) override;
   const std::string & getCurrentAction() const override;
 
@@ -79,7 +79,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(double current_time, double step_time);
+  BT::NodeStatus tickOnce(const double current_time, const double step_time);
   auto createBehaviorTree(const std::string & format_path) -> BT::Tree;
   BT::BehaviorTreeFactory factory_;
   BT::Tree tree_;
diff --git a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
index bc083253ca0..05931da9f40 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
@@ -83,7 +83,7 @@ const std::string & PedestrianBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void PedestrianBehaviorTree::update(double current_time, double step_time)
+void PedestrianBehaviorTree::update(const double current_time, const double step_time)
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
@@ -91,7 +91,7 @@ void PedestrianBehaviorTree::update(double current_time, double step_time)
   }
 }
 
-BT::NodeStatus PedestrianBehaviorTree::tickOnce(double current_time, double step_time)
+BT::NodeStatus PedestrianBehaviorTree::tickOnce(const double current_time, const double step_time)
 {
   setCurrentTime(current_time);
   setStepTime(step_time);
diff --git a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
index 4a9e22c1c56..639920828c1 100644
--- a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
+++ b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
@@ -142,7 +142,7 @@ const std::string & VehicleBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void VehicleBehaviorTree::update(double current_time, double step_time)
+void VehicleBehaviorTree::update(const double current_time, const double step_time)
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
@@ -150,7 +150,7 @@ void VehicleBehaviorTree::update(double current_time, double step_time)
   }
 }
 
-BT::NodeStatus VehicleBehaviorTree::tickOnce(double current_time, double step_time)
+BT::NodeStatus VehicleBehaviorTree::tickOnce(const double current_time, const double step_time)
 {
   setCurrentTime(current_time);
   setStepTime(step_time);
diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
index 8094598791c..b1610512573 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
@@ -41,7 +41,7 @@ class BehaviorPluginBase
 public:
   virtual ~BehaviorPluginBase() = default;
   virtual void configure(const rclcpp::Logger & logger) = 0;
-  virtual void update(double current_time, double step_time) = 0;
+  virtual void update(const double current_time, const double step_time) = 0;
   virtual const std::string & getCurrentAction() const = 0;
 
 #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE)      \
diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp
index 79d3d516699..95df82b7990 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp
@@ -26,7 +26,7 @@ namespace longitudinal_speed_planning
 class LongitudinalSpeedPlanner
 {
 public:
-  explicit LongitudinalSpeedPlanner(double step_time, const std::string & entity);
+  explicit LongitudinalSpeedPlanner(const double step_time, const std::string & entity);
   auto getDynamicStates(
     double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &,
     const geometry_msgs::msg::Twist & current_twist,
diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
index 77adc314c72..9eb897e6488 100644
--- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
+++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -47,7 +47,7 @@ auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus & entity_status,
   traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory,
   const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils, double step_time,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils, const double step_time,
   std::optional<double> target_speed) -> std::optional<EntityStatus>
 {
   using math::arithmetic::isApproximatelyEqualTo;
diff --git a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
index bb49afa620e..5d875138580 100644
--- a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
+++ b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp
@@ -23,7 +23,8 @@ namespace traffic_simulator
 {
 namespace longitudinal_speed_planning
 {
-LongitudinalSpeedPlanner::LongitudinalSpeedPlanner(double step_time, const std::string & entity)
+LongitudinalSpeedPlanner::LongitudinalSpeedPlanner(
+  const double step_time, const std::string & entity)
 : step_time(step_time), entity(entity)
 {
 }

From 93faa807aa8180db7bce2ba8327a7a7af56c7bae Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Tue, 4 Jun 2024 15:58:53 +0200
Subject: [PATCH 08/12] Correct style - add else

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 simulation/traffic_simulator/src/entity/ego_entity.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index ccc5ad7b5db..6109a3fbe03 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -274,13 +274,14 @@ auto EgoEntity::setBehaviorParameter(
   behavior_parameter_ = behavior_parameter;
 }
 
-void EgoEntity::requestSpeedChange(double value, bool)
+void EgoEntity::requestSpeedChange(double value, bool /* continuous */)
 {
   if (status_.getTime() > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
+  } else {
+    target_speed_ = value;
+    field_operator_application->restrictTargetSpeed(value);
   }
-  target_speed_ = value;
-  field_operator_application->restrictTargetSpeed(value);
 }
 
 void EgoEntity::requestSpeedChange(

From 022c4ffb9cbd50a00c13532dc1fee4c03a76d75e Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Tue, 4 Jun 2024 16:29:22 +0200
Subject: [PATCH 09/12] Trailing return type

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../behavior_tree_plugin/pedestrian/behavior_tree.hpp     | 4 ++--
 .../behavior_tree_plugin/vehicle/behavior_tree.hpp        | 4 ++--
 .../behavior_tree_plugin/src/pedestrian/behavior_tree.cpp | 5 +++--
 .../behavior_tree_plugin/src/vehicle/behavior_tree.cpp    | 5 +++--
 .../traffic_simulator/behavior/behavior_plugin_base.hpp   | 2 +-
 .../include/traffic_simulator/entity/ego_entity.hpp       | 2 +-
 .../include/traffic_simulator/entity/entity_base.hpp      | 4 ++--
 .../traffic_simulator/entity/pedestrian_entity.hpp        | 2 +-
 .../include/traffic_simulator/entity/vehicle_entity.hpp   | 2 +-
 simulation/traffic_simulator/src/entity/ego_entity.cpp    | 8 ++++----
 simulation/traffic_simulator/src/entity/entity_base.cpp   | 4 ++--
 .../traffic_simulator/src/entity/misc_object_entity.cpp   | 2 +-
 .../traffic_simulator/src/entity/pedestrian_entity.cpp    | 2 +-
 .../traffic_simulator/src/entity/vehicle_entity.cpp       | 2 +-
 14 files changed, 25 insertions(+), 23 deletions(-)

diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
index d82d5375c25..8f94cad2a07 100644
--- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
+++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp
@@ -39,7 +39,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 {
 public:
   void configure(const rclcpp::Logger & logger) override;
-  void update(const double current_time, const double step_time) override;
+  auto update(const double current_time, const double step_time) -> void override;
   const std::string & getCurrentAction() const override;
 
 #define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
@@ -76,7 +76,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(const double current_time, const double step_time);
+  auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus;
   auto createBehaviorTree(const std::string & format_path) -> BT::Tree;
   BT::BehaviorTreeFactory factory_;
   BT::Tree tree_;
diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
index 219a3b27743..7789a28d9be 100644
--- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
+++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp
@@ -38,7 +38,7 @@ namespace entity_behavior
 class VehicleBehaviorTree : public BehaviorPluginBase
 {
 public:
-  void update(const double current_time, const double step_time) override;
+  auto update(const double current_time, const double step_time) -> void override;
   void configure(const rclcpp::Logger & logger) override;
   const std::string & getCurrentAction() const override;
 
@@ -79,7 +79,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(const double current_time, const double step_time);
+  auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus;
   auto createBehaviorTree(const std::string & format_path) -> BT::Tree;
   BT::BehaviorTreeFactory factory_;
   BT::Tree tree_;
diff --git a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
index 05931da9f40..4cfaf3a8ea5 100644
--- a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
+++ b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp
@@ -83,7 +83,7 @@ const std::string & PedestrianBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void PedestrianBehaviorTree::update(const double current_time, const double step_time)
+auto PedestrianBehaviorTree::update(const double current_time, const double step_time) -> void
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
@@ -91,7 +91,8 @@ void PedestrianBehaviorTree::update(const double current_time, const double step
   }
 }
 
-BT::NodeStatus PedestrianBehaviorTree::tickOnce(const double current_time, const double step_time)
+auto PedestrianBehaviorTree::tickOnce(const double current_time, const double step_time)
+  -> BT::NodeStatus
 {
   setCurrentTime(current_time);
   setStepTime(step_time);
diff --git a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
index 639920828c1..fbb49c65d63 100644
--- a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
+++ b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp
@@ -142,7 +142,7 @@ const std::string & VehicleBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void VehicleBehaviorTree::update(const double current_time, const double step_time)
+auto VehicleBehaviorTree::update(const double current_time, const double step_time) -> void
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
@@ -150,7 +150,8 @@ void VehicleBehaviorTree::update(const double current_time, const double step_ti
   }
 }
 
-BT::NodeStatus VehicleBehaviorTree::tickOnce(const double current_time, const double step_time)
+auto VehicleBehaviorTree::tickOnce(const double current_time, const double step_time)
+  -> BT::NodeStatus
 {
   setCurrentTime(current_time);
   setStepTime(step_time);
diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
index b1610512573..a18d82da316 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp
@@ -41,7 +41,7 @@ class BehaviorPluginBase
 public:
   virtual ~BehaviorPluginBase() = default;
   virtual void configure(const rclcpp::Logger & logger) = 0;
-  virtual void update(const double current_time, const double step_time) = 0;
+  virtual auto update(const double current_time, const double step_time) -> void = 0;
   virtual const std::string & getCurrentAction() const = 0;
 
 #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE)      \
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
index b38389b094c..997e4f9a642 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
@@ -85,7 +85,7 @@ class EgoEntity : public VehicleEntity
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(const double current_time, const double step_time) override;
+  auto onUpdate(const double current_time, const double step_time) -> void override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &) override;
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
index 26a7aa1539a..911a6afc18a 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -124,9 +124,9 @@ class EntityBase
 
   virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
 
-  virtual void onUpdate(const double current_time, const double step_time);
+  virtual auto onUpdate(const double current_time, const double step_time) -> void;
 
-  virtual void onPostUpdate(const double current_time, const double step_time);
+  virtual auto onPostUpdate(const double current_time, const double step_time) -> void;
 
   /*   */ void resetDynamicConstraints();
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
index cc46cb3374d..c152248997e 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
@@ -70,7 +70,7 @@ class PedestrianEntity : public EntityBase
 
   auto getEntityTypename() const -> const std::string & override;
 
-  void onUpdate(const double current_time, const double step_time) override;
+  auto onUpdate(const double current_time, const double step_time) -> void override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) override;
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
index 04ec4577637..a8ee76dd248 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp
@@ -92,7 +92,7 @@ class VehicleEntity : public EntityBase
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(const double current_time, const double step_time) override;
+  auto onUpdate(const double current_time, const double step_time) -> void override;
 
   void requestAcquirePosition(const CanonicalizedLaneletPose &);
 
diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index 6109a3fbe03..5850cf49f0d 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -148,7 +148,7 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr
   return field_operator_application->getWaypoints();
 }
 
-void EgoEntity::onUpdate(const double current_time, const double step_time)
+auto EgoEntity::onUpdate(const double current_time, const double step_time) -> void
 {
   EntityBase::onUpdate(current_time, step_time);
 
@@ -274,7 +274,7 @@ auto EgoEntity::setBehaviorParameter(
   behavior_parameter_ = behavior_parameter;
 }
 
-void EgoEntity::requestSpeedChange(double value, bool /* continuous */)
+auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void
 {
   if (status_.getTime() > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
@@ -284,8 +284,8 @@ void EgoEntity::requestSpeedChange(double value, bool /* continuous */)
   }
 }
 
-void EgoEntity::requestSpeedChange(
-  const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/)
+auto EgoEntity::requestSpeedChange(
+  const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/) -> void
 {
   THROW_SEMANTIC_ERROR(
     "The traffic_simulator's request to set speed to the Ego type entity is for initialization "
diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
index 9d37de608fd..7b0fbf463dc 100644
--- a/simulation/traffic_simulator/src/entity/entity_base.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -99,7 +99,7 @@ auto EntityBase::isTargetSpeedReached(const speed_change::RelativeTargetSpeed &
   return isTargetSpeedReached(target_speed.getAbsoluteValue(getStatus(), other_status_));
 }
 
-void EntityBase::onUpdate(const double /*current_time*/, const double step_time)
+auto EntityBase::onUpdate(const double /*current_time*/, const double step_time) -> void
 {
   job_list_.update(step_time, job::Event::PRE_UPDATE);
   status_before_update_.set(status_);
@@ -108,7 +108,7 @@ void EntityBase::onUpdate(const double /*current_time*/, const double step_time)
       step_time, name);
 }
 
-void EntityBase::onPostUpdate(const double /*current_time*/, const double step_time)
+auto EntityBase::onPostUpdate(const double /*current_time*/, const double step_time) -> void
 {
   job_list_.update(step_time, job::Event::POST_UPDATE);
 }
diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
index cc60bc2ac90..16b0e3bde6c 100644
--- a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
@@ -26,7 +26,7 @@ MiscObjectEntity::MiscObjectEntity(
 {
 }
 
-void MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time)
+auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time) -> void
 {
   setTwist(geometry_msgs::msg::Twist());
   setAcceleration(geometry_msgs::msg::Accel());
diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
index 5f8572d6203..385e61c39c1 100644
--- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
@@ -248,7 +248,7 @@ void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate)
   setBehaviorParameter(behavior_parameter);
 }
 
-void PedestrianEntity::onUpdate(const double current_time, const double step_time)
+auto PedestrianEntity::onUpdate(const double current_time, const double step_time) -> void
 {
   EntityBase::onUpdate(current_time, step_time);
   if (npc_logic_started_) {
diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
index f6278f79000..73d0a7c833c 100644
--- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -139,7 +139,7 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin
   }
 }
 
-void VehicleEntity::onUpdate(const double current_time, const double step_time)
+auto VehicleEntity::onUpdate(const double current_time, const double step_time) -> void
 {
   EntityBase::onUpdate(current_time, step_time);
   if (npc_logic_started_) {

From 8158b6a463346f8a41798d70cd73217bddc5a546 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Wed, 12 Jun 2024 22:27:54 +0200
Subject: [PATCH 10/12] Resolve conflicts with stored time

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../include/traffic_simulator/entity/entity_manager.hpp       | 2 +-
 simulation/traffic_simulator/src/entity/entity_manager.cpp    | 4 +---
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
index cb4ae5108cd..534f67e49e8 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -472,7 +472,7 @@ class EntityManager
       // FIXME: this ignores V2I traffic lights
       iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_);
       if (npc_logic_started_ && not is<EgoEntity>(name)) {
-        iter->second->startNpcLogic(getCurrentTime());
+        iter->second->startNpcLogic(current_time);
       }
       return success;
     } else {
diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp
index 53e02889e87..b4968b6068d 100644
--- a/simulation/traffic_simulator/src/entity/entity_manager.cpp
+++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp
@@ -426,10 +426,8 @@ void EntityManager::startNpcLogic(const double current_time)
 {
   npc_logic_started_ = true;
 
-  current_time_ = current_time;
-
   for ([[maybe_unused]] auto && [name, entity] : entities_) {
-    entity->startNpcLogic(current_time_);
+    entity->startNpcLogic(current_time);
   }
 }
 }  // namespace entity

From 836540f81b2b01cae1e493e2aeecfb00836247d1 Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Tue, 27 Aug 2024 18:13:57 +0200
Subject: [PATCH 11/12] Fix after using pointer to store entity status

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
index 8a3dba65a4d..02ab5ff4ef3 100644
--- a/simulation/traffic_simulator/src/entity/ego_entity.cpp
+++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -268,7 +268,7 @@ auto EgoEntity::setBehaviorParameter(
 
 auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void
 {
-  if (status_.getTime() > 0.0) {
+  if (status_->getTime() > 0.0) {
     THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario.");
   } else {
     target_speed_ = value;

From 10fbcd9a2a91e419755d6790fe60f8bc9c94dc4d Mon Sep 17 00:00:00 2001
From: Release Bot <action@github.com>
Date: Wed, 28 Aug 2024 10:01:24 +0000
Subject: [PATCH 12/12] Bump version of scenario_simulator_v2 from version
 4.0.1 to version 4.0.2

---
 common/math/arithmetic/CHANGELOG.rst          | 12 +++++++++
 common/math/arithmetic/package.xml            |  2 +-
 common/math/geometry/CHANGELOG.rst            | 12 +++++++++
 common/math/geometry/package.xml              |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../scenario_simulator_exception/package.xml  |  2 +-
 common/simple_junit/CHANGELOG.rst             | 12 +++++++++
 common/simple_junit/package.xml               |  2 +-
 common/status_monitor/CHANGELOG.rst           | 12 +++++++++
 common/status_monitor/package.xml             |  2 +-
 external/concealer/CHANGELOG.rst              | 12 +++++++++
 external/concealer/package.xml                |  2 +-
 external/embree_vendor/CHANGELOG.rst          | 12 +++++++++
 external/embree_vendor/package.xml            |  2 +-
 map/kashiwanoha_map/CHANGELOG.rst             | 12 +++++++++
 map/kashiwanoha_map/package.xml               |  2 +-
 map/simple_cross_map/CHANGELOG.rst            |  8 ++++++
 map/simple_cross_map/package.xml              |  2 +-
 mock/cpp_mock_scenarios/CHANGELOG.rst         | 12 +++++++++
 mock/cpp_mock_scenarios/package.xml           |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../package.xml                               |  2 +-
 .../openscenario_interpreter/CHANGELOG.rst    | 12 +++++++++
 .../openscenario_interpreter/package.xml      |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../package.xml                               |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../openscenario_interpreter_msgs/package.xml |  2 +-
 .../openscenario_preprocessor/CHANGELOG.rst   | 12 +++++++++
 .../openscenario_preprocessor/package.xml     |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../package.xml                               |  2 +-
 .../openscenario_utility/CHANGELOG.rst        | 12 +++++++++
 openscenario/openscenario_utility/package.xml |  2 +-
 .../openscenario_validator/CHANGELOG.rst      | 12 +++++++++
 .../openscenario_validator/package.xml        |  2 +-
 .../openscenario_visualization/CHANGELOG.rst  | 12 +++++++++
 .../openscenario_visualization/package.xml    |  2 +-
 .../CHANGELOG.rst                             | 12 +++++++++
 .../package.xml                               |  2 +-
 scenario_simulator_v2/CHANGELOG.rst           | 12 +++++++++
 scenario_simulator_v2/package.xml             |  2 +-
 simulation/behavior_tree_plugin/CHANGELOG.rst | 16 ++++++++++++
 simulation/behavior_tree_plugin/package.xml   |  2 +-
 simulation/do_nothing_plugin/CHANGELOG.rst    | 12 +++++++++
 simulation/do_nothing_plugin/package.xml      |  2 +-
 .../simple_sensor_simulator/CHANGELOG.rst     | 12 +++++++++
 .../simple_sensor_simulator/package.xml       |  2 +-
 simulation/simulation_interface/CHANGELOG.rst | 12 +++++++++
 simulation/simulation_interface/package.xml   |  2 +-
 simulation/traffic_simulator/CHANGELOG.rst    | 26 +++++++++++++++++++
 simulation/traffic_simulator/package.xml      |  2 +-
 .../traffic_simulator_msgs/CHANGELOG.rst      | 12 +++++++++
 simulation/traffic_simulator_msgs/package.xml |  2 +-
 test_runner/random_test_runner/CHANGELOG.rst  | 12 +++++++++
 test_runner/random_test_runner/package.xml    |  2 +-
 .../scenario_test_runner/CHANGELOG.rst        | 12 +++++++++
 test_runner/scenario_test_runner/package.xml  |  2 +-
 58 files changed, 391 insertions(+), 29 deletions(-)

diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst
index f0cf2ec7855..3eea6a621bf 100644
--- a/common/math/arithmetic/CHANGELOG.rst
+++ b/common/math/arithmetic/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package arithmetic
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml
index 3625943a4d7..e1ae74a387f 100644
--- a/common/math/arithmetic/package.xml
+++ b/common/math/arithmetic/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>arithmetic</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>arithmetic library for scenario_simulator_v2</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst
index 611fb339998..cea9d9379a5 100644
--- a/common/math/geometry/CHANGELOG.rst
+++ b/common/math/geometry/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package geometry
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml
index 87539646efd..ee62a7ba900 100644
--- a/common/math/geometry/package.xml
+++ b/common/math/geometry/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>geometry</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>geometry math library for scenario_simulator_v2 application</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst
index 5b706727192..4de288d7dca 100644
--- a/common/scenario_simulator_exception/CHANGELOG.rst
+++ b/common/scenario_simulator_exception/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package scenario_simulator_exception
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml
index 8daac31a5d8..3573be7471a 100644
--- a/common/scenario_simulator_exception/package.xml
+++ b/common/scenario_simulator_exception/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>scenario_simulator_exception</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Exception types for scenario simulator</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst
index a1aaf142434..d6d8048199f 100644
--- a/common/simple_junit/CHANGELOG.rst
+++ b/common/simple_junit/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package junit_exporter
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml
index 459a77350d7..aa67db709cf 100644
--- a/common/simple_junit/package.xml
+++ b/common/simple_junit/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_junit</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Lightweight JUnit library for ROS 2</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst
index 34e1cfdb571..f784c5c6236 100644
--- a/common/status_monitor/CHANGELOG.rst
+++ b/common/status_monitor/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package status_monitor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml
index 36e7e7bb9cc..b9a66438d63 100644
--- a/common/status_monitor/package.xml
+++ b/common/status_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>status_monitor</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>none</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst
index bdb4a51fba2..ed3a6c4afa3 100644
--- a/external/concealer/CHANGELOG.rst
+++ b/external/concealer/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package concealer
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/external/concealer/package.xml b/external/concealer/package.xml
index 73b37aa2a0b..7e314b8d3fb 100644
--- a/external/concealer/package.xml
+++ b/external/concealer/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>concealer</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator.</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst
index 21b51ed81e4..1a8b4a358ff 100644
--- a/external/embree_vendor/CHANGELOG.rst
+++ b/external/embree_vendor/CHANGELOG.rst
@@ -24,6 +24,18 @@ Changelog for package embree_vendor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml
index 58f9588ef97..0f3175ec285 100644
--- a/external/embree_vendor/package.xml
+++ b/external/embree_vendor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>embree_vendor</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>vendor packages for intel raytracing kernel library</description>
   <maintainer email="ms.kataoka@gmail.com">masaya</maintainer>
   <license>Apache 2.0</license>
diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst
index cfc1b88fe46..fca9ead920c 100644
--- a/map/kashiwanoha_map/CHANGELOG.rst
+++ b/map/kashiwanoha_map/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package kashiwanoha_map
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml
index 9dd1feb8811..ce5a22479d0 100644
--- a/map/kashiwanoha_map/package.xml
+++ b/map/kashiwanoha_map/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kashiwanoha_map</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>map package for kashiwanoha</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst
index 7e0c7ebe807..d534602af7c 100644
--- a/map/simple_cross_map/CHANGELOG.rst
+++ b/map/simple_cross_map/CHANGELOG.rst
@@ -9,6 +9,14 @@ Changelog for package simple_cross_map
 * Merge branch 'master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml
index e677dc10515..7ec741ec654 100644
--- a/map/simple_cross_map/package.xml
+++ b/map/simple_cross_map/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_cross_map</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>map package for simple cross</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst
index 04522f2e6a5..49e4d9aa494 100644
--- a/mock/cpp_mock_scenarios/CHANGELOG.rst
+++ b/mock/cpp_mock_scenarios/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package cpp_mock_scenarios
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml
index 6b6bd72812f..fc798c3714c 100644
--- a/mock/cpp_mock_scenarios/package.xml
+++ b/mock/cpp_mock_scenarios/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cpp_mock_scenarios</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>C++ mock scenarios</description>
   <maintainer email="masaya.kataoka@tier4.jp">masaya</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst
index 0cc316e9b5d..985452638a4 100644
--- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst
+++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_experimental_catalog
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml
index e57ddb23587..915d879417b 100644
--- a/openscenario/openscenario_experimental_catalog/package.xml
+++ b/openscenario/openscenario_experimental_catalog/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_experimental_catalog</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>TIER IV experimental catalogs for OpenSCENARIO</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst
index e241f116058..24bc48c0316 100644
--- a/openscenario/openscenario_interpreter/CHANGELOG.rst
+++ b/openscenario/openscenario_interpreter/CHANGELOG.rst
@@ -32,6 +32,18 @@ Changelog for package openscenario_interpreter
 * add publish_empty_context parameter
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml
index a10be27cf5f..aa546ccfbf3 100644
--- a/openscenario/openscenario_interpreter/package.xml
+++ b/openscenario/openscenario_interpreter/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_interpreter</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>OpenSCENARIO 1.2.0 interpreter package for Autoware</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst
index 073fc6723e6..21ac7a73a13 100644
--- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst
+++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_example
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml
index ce15c7e10e6..9216e5aec59 100644
--- a/openscenario/openscenario_interpreter_example/package.xml
+++ b/openscenario/openscenario_interpreter_example/package.xml
@@ -3,7 +3,7 @@
 
 <package format="3">
   <name>openscenario_interpreter_example</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Examples for some TIER IV OpenSCENARIO Interpreter's features</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst
index 75b6a57ddc1..a944ec8b220 100644
--- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst
+++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_msgs
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml
index ec4cbe4de0c..72051b931b5 100644
--- a/openscenario/openscenario_interpreter_msgs/package.xml
+++ b/openscenario/openscenario_interpreter_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_interpreter_msgs</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>ROS message types for package openscenario_interpreter</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Yamasaki Tatsuya</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst
index d432795c645..ee268d19bdf 100644
--- a/openscenario/openscenario_preprocessor/CHANGELOG.rst
+++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml
index b143a04b9cc..2a1e3cca1ec 100644
--- a/openscenario/openscenario_preprocessor/package.xml
+++ b/openscenario/openscenario_preprocessor/package.xml
@@ -3,7 +3,7 @@
 
 <package format="3">
   <name>openscenario_preprocessor</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Example package for TIER IV OpenSCENARIO Interpreter</description>
   <maintainer email="kotaro.yoshimoto@tier4.jp">Kotaro Yoshimoto</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst
index 486258b6f6e..2f78e66db2d 100644
--- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst
+++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor_msgs
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml
index 49c22a096e1..6d098c0525e 100644
--- a/openscenario/openscenario_preprocessor_msgs/package.xml
+++ b/openscenario/openscenario_preprocessor_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_preprocessor_msgs</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>ROS message types for package openscenario_preprocessor</description>
   <maintainer email="kotaro.yoshimoto@tier4.jp">Kotaro Yoshimoto</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst
index 9e6a0cff55c..87a358bbb86 100644
--- a/openscenario/openscenario_utility/CHANGELOG.rst
+++ b/openscenario/openscenario_utility/CHANGELOG.rst
@@ -24,6 +24,18 @@ Changelog for package openscenario_utility
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml
index bddd78b888a..fd2a6711791 100644
--- a/openscenario/openscenario_utility/package.xml
+++ b/openscenario/openscenario_utility/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_utility</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Utility tools for ASAM OpenSCENARIO 1.2.0</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>
diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst
index dbc02ffa15e..5149084599a 100644
--- a/openscenario/openscenario_validator/CHANGELOG.rst
+++ b/openscenario/openscenario_validator/CHANGELOG.rst
@@ -10,6 +10,18 @@ Changelog for package openscenario_validator
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml
index 727d84b215e..42b58938422 100644
--- a/openscenario/openscenario_validator/package.xml
+++ b/openscenario/openscenario_validator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_validator</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Validator for OpenSCENARIO 1.3</description>
   <maintainer email="kotaro.yoshimoto@tier4.jp">Kotaro Yoshimoto</maintainer>
   <license>Apache License 2.0</license>
diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst
index e9030b9e398..f46fd32c6de 100644
--- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst
+++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_visualization
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml
index bf087c6d752..9964516dfe8 100644
--- a/rviz_plugins/openscenario_visualization/package.xml
+++ b/rviz_plugins/openscenario_visualization/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>openscenario_visualization</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Visualization tools for simulation results</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst
index 76196479a03..14e511cde28 100644
--- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst
+++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package real_time_factor_control_rviz_plugin
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml
index a85e7d945cb..ba5353a86a1 100644
--- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml
+++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>real_time_factor_control_rviz_plugin</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Slider controlling real time factor value.</description>
   <maintainer email="pawel.lech@robotec.ai">Paweł Lech</maintainer>
   <license>Apache License 2.0</license>
diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst
index f4b6cded252..f59f51e5e8d 100644
--- a/scenario_simulator_v2/CHANGELOG.rst
+++ b/scenario_simulator_v2/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package scenario_simulator_v2
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml
index 341ff2f1148..8a5d2fbfdf0 100644
--- a/scenario_simulator_v2/package.xml
+++ b/scenario_simulator_v2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>scenario_simulator_v2</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>scenario testing framework for Autoware</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst
index d626c620326..e22038f6eb4 100644
--- a/simulation/behavior_tree_plugin/CHANGELOG.rst
+++ b/simulation/behavior_tree_plugin/CHANGELOG.rst
@@ -21,6 +21,22 @@ Changelog for package behavior_tree_plugin
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge pull request `#1356 <https://github.com/tier4/scenario_simulator_v2/issues/1356>`_ from tier4/RJD-1056-remove-current-time-step-time
+  Remove unused data members: current_time step_time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Trailing return type
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Add const to time argument in behavior
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml
index 73653dde912..e0cec8884ff 100644
--- a/simulation/behavior_tree_plugin/package.xml
+++ b/simulation/behavior_tree_plugin/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>behavior_tree_plugin</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Behavior tree plugin for traffic_simulator</description>
   <maintainer email="masaya.kataoka@tier4.jp">masaya</maintainer>
   <license>Apache 2.0</license>
diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst
index 3887770331c..6b90ad05182 100644
--- a/simulation/do_nothing_plugin/CHANGELOG.rst
+++ b/simulation/do_nothing_plugin/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package do_nothing_plugin
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge pull request `#1354 <https://github.com/tier4/scenario_simulator_v2/issues/1354>`_ from tier4/fix/follow_trajectory
diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml
index 532f61a26a1..bc4a3caa870 100644
--- a/simulation/do_nothing_plugin/package.xml
+++ b/simulation/do_nothing_plugin/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>do_nothing_plugin</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Behavior plugin for do nothing</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache 2.0</license>
diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst
index ff982390614..ded1c84e89d 100644
--- a/simulation/simple_sensor_simulator/CHANGELOG.rst
+++ b/simulation/simple_sensor_simulator/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package simple_sensor_simulator
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml
index ca7365c68ce..5781bd1431d 100644
--- a/simulation/simple_sensor_simulator/package.xml
+++ b/simulation/simple_sensor_simulator/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>simple_sensor_simulator</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>simple_sensor_simulator package</description>
 
   <maintainer email="masaya.kataoka@tier4.jp">masaya kataoka</maintainer>
diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst
index 54263df3416..49ea5583645 100644
--- a/simulation/simulation_interface/CHANGELOG.rst
+++ b/simulation/simulation_interface/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package simulation_interface
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml
index 3b64b4a5d50..46a303c53ca 100644
--- a/simulation/simulation_interface/package.xml
+++ b/simulation/simulation_interface/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simulation_interface</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>packages to define interface between simulator and scenario interpreter</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst
index 1187b626236..398fa88980a 100644
--- a/simulation/traffic_simulator/CHANGELOG.rst
+++ b/simulation/traffic_simulator/CHANGELOG.rst
@@ -21,6 +21,32 @@ Changelog for package traffic_simulator
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge pull request `#1356 <https://github.com/tier4/scenario_simulator_v2/issues/1356>`_ from tier4/RJD-1056-remove-current-time-step-time
+  Remove unused data members: current_time step_time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Fix after using pointer to store entity status
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Resolve conflicts with stored time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Trailing return type
+* Correct style - add else
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Add const to time argument in behavior
+* Add const to time argument
+* Use member instead of getter
+* Revert to previous macro definition
+* Move requestSpeedChange time check responsibility to EgoEntity and simplify
+* Correct style
+* Remove step_time\_ and current_time\_ data members from EntityManager
+  Adjust the code so that the time is managed in API only
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge pull request `#1354 <https://github.com/tier4/scenario_simulator_v2/issues/1354>`_ from tier4/fix/follow_trajectory
diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml
index aa42fda39d0..f638daa47b4 100644
--- a/simulation/traffic_simulator/package.xml
+++ b/simulation/traffic_simulator/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>traffic_simulator</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>control traffic flow</description>
 
   <maintainer email="masaya.kataoka@tier4.jp">masaya kataoka</maintainer>
diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst
index acdd6da5397..cd135290fc0 100644
--- a/simulation/traffic_simulator_msgs/CHANGELOG.rst
+++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package openscenario_msgs
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml
index a5c656b0110..308f10ff0d4 100644
--- a/simulation/traffic_simulator_msgs/package.xml
+++ b/simulation/traffic_simulator_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>traffic_simulator_msgs</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>ROS messages for openscenario</description>
   <maintainer email="masaya.kataoka@tier4.jp">Masaya Kataoka</maintainer>
   <license>Apache License 2.0</license>
diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst
index d55240d8255..ecf12a30aba 100644
--- a/test_runner/random_test_runner/CHANGELOG.rst
+++ b/test_runner/random_test_runner/CHANGELOG.rst
@@ -21,6 +21,18 @@ Changelog for package random_test_runner
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml
index 19052431620..27cd06d1d4a 100644
--- a/test_runner/random_test_runner/package.xml
+++ b/test_runner/random_test_runner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>random_test_runner</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>Random behavior test runner</description>
   <maintainer email="piotr-zyskowski@robotec.ai">piotr-zyskowski-rai</maintainer>
   <license>Apache License 2.0</license>
diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst
index 3334ed01d30..715fda21fa8 100644
--- a/test_runner/scenario_test_runner/CHANGELOG.rst
+++ b/test_runner/scenario_test_runner/CHANGELOG.rst
@@ -35,6 +35,18 @@ Changelog for package scenario_test_runner
 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context
 * Contributors: Masaya Kataoka
 
+4.0.2 (2024-08-28)
+------------------
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'master' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time
+* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk
+
 4.0.1 (2024-08-28)
 ------------------
 * Merge branch 'master' into fix/follow_trajectory
diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml
index ad38e922e42..df4572a6dab 100644
--- a/test_runner/scenario_test_runner/package.xml
+++ b/test_runner/scenario_test_runner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>scenario_test_runner</name>
-  <version>4.0.1</version>
+  <version>4.0.2</version>
   <description>scenario test runner package</description>
   <maintainer email="tatsuya.yamasaki@tier4.jp">Tatsuya Yamasaki</maintainer>
   <license>Apache License 2.0</license>