Register StandStillDuration to job list

tier4 · Oct 2, 2024 · 3c82da8 · 3c82da8
1 parent 5602645
commit 3c82da8
Show file tree

Hide file tree

Showing 7 changed files with 12 additions and 29 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -222,8 +222,6 @@ class EntityBase
 
   /*   */ void updateEntityStatusTimestamp(const double current_time);
 
-  /*   */ auto updateStandStillDuration(const double step_time) -> double;
-
   /*   */ auto updateTraveledDistance(const double step_time) -> double;
 
   /*   */ bool reachPosition(

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -165,7 +165,6 @@ void EgoEntity::onUpdate(double current_time, double step_time)
     updateEntityStatusTimestamp(current_time + step_time);
   }
 
-  updateStandStillDuration(step_time);
   updateTraveledDistance(step_time);
   updateFieldOperatorApplication();
 

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -45,6 +45,17 @@ EntityBase::EntityBase(
       " and the name of the entity listed in entity_status is ",
       static_cast<EntityStatus>(entity_status).name);
   }
+
+  job_list_.append(
+    [this](double delta_time) {
+      if (std::abs(getCurrentTwist().linear.x) <= std::numeric_limits<double>::epsilon()) {
+        stand_still_duration_ += delta_time;
+      } else {
+        stand_still_duration_ = 0.0;
+      }
+      return false;
+    },
+    [this]() {}, job::Type::STAND_STILL_DURATION, true, job::Event::POST_UPDATE);
 }
 
 void EntityBase::appendDebugMarker(visualization_msgs::msg::MarkerArray &) {}
@@ -630,15 +641,6 @@ void EntityBase::updateEntityStatusTimestamp(const double current_time)
   status_->setTime(current_time);
 }
 
-auto EntityBase::updateStandStillDuration(const double step_time) -> double
-{
-  if (std::abs(getCurrentTwist().linear.x) <= std::numeric_limits<double>::epsilon()) {
-    return stand_still_duration_ += step_time;
-  } else {
-    return stand_still_duration_ = 0.0;
-  }
-}
-
 auto EntityBase::updateTraveledDistance(const double step_time) -> double
 {
   return traveled_distance_ += std::abs(getCurrentTwist().linear.x) * step_time;

diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp
@@ -26,13 +26,12 @@ MiscObjectEntity::MiscObjectEntity(
 {
 }
 
-auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time) -> void
+auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double ) -> void
 {
   setTwist(geometry_msgs::msg::Twist());
   setAcceleration(geometry_msgs::msg::Accel());
   setLinearJerk(0.0);
   setAction("static");
-  updateStandStillDuration(step_time);
   status_before_update_.set(*status_);
 }
 

diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
@@ -258,12 +258,10 @@ auto PedestrianEntity::onUpdate(const double current_time, const double step_tim
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) {
       stopAtCurrentPosition();
-      updateStandStillDuration(step_time);
       updateTraveledDistance(step_time);
       return;
     }
   }
-  updateStandStillDuration(step_time);
   updateTraveledDistance(step_time);
 
   EntityBase::onPostUpdate(current_time, step_time);

diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -150,12 +150,10 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time)
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) {
       stopAtCurrentPosition();
-      updateStandStillDuration(step_time);
       updateTraveledDistance(step_time);
       return;
     }
   }
-  updateStandStillDuration(step_time);
   updateTraveledDistance(step_time);
 
   EntityBase::onPostUpdate(current_time, step_time);

diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp
@@ -438,17 +438,6 @@ TEST_F(MiscObjectEntityTest_FullObject, requestWalkStraight)
   EXPECT_THROW(misc_object.requestWalkStraight(), common::Error);
 }
 
-/**
- * @note test basic functionality; test updating stand still duration
- * when NPC logic is started and velocity is greater than 0.
- */
-TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_startedMoving)
-{
-  misc_object.setLinearVelocity(3.0);
-
-  EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1));
-}
-
 /**
  * @note Test basic functionality; test updating traveled distance correctness
  * with NPC logic started and velocity greater than 0.