Merge pull request #1356 from tier4/RJD-1056-remove-current-time-step…

…-time Remove unused data members: current_time step_time
tier4 · Aug 28, 2024 · 7a5afbb · 7a5afbb
2 parents 5324a67 + 361eb4c
commit 7a5afbb
Show file tree

Hide file tree

Showing 20 changed files with 60 additions and 114 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
@@ -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;
+  auto update(const double current_time, const double step_time) -> void override;
   const std::string & getCurrentAction() const override;
 
 #define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
@@ -75,7 +75,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(double current_time, 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
@@ -38,7 +38,7 @@ namespace entity_behavior
 class VehicleBehaviorTree : public BehaviorPluginBase
 {
 public:
-  void update(double current_time, 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;
 
@@ -78,7 +78,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
 #undef DEFINE_GETTER_SETTER
 
 private:
-  BT::NodeStatus tickOnce(double current_time, 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
@@ -83,15 +83,16 @@ const std::string & PedestrianBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void PedestrianBehaviorTree::update(double current_time, double step_time)
+auto PedestrianBehaviorTree::update(const double current_time, const double step_time) -> void
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
     tickOnce(current_time, step_time);
   }
 }
 
-BT::NodeStatus PedestrianBehaviorTree::tickOnce(double current_time, 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
@@ -142,15 +142,16 @@ const std::string & VehicleBehaviorTree::getCurrentAction() const
   return logging_event_ptr_->getCurrentAction();
 }
 
-void VehicleBehaviorTree::update(double current_time, double step_time)
+auto VehicleBehaviorTree::update(const double current_time, const double step_time) -> void
 {
   tickOnce(current_time, step_time);
   while (getCurrentAction() == "root") {
     tickOnce(current_time, step_time);
   }
 }
 
-BT::NodeStatus VehicleBehaviorTree::tickOnce(double current_time, 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/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -146,10 +146,10 @@ class API
       if (behavior == VehicleBehavior::autoware()) {
         return entity_manager_ptr_->entityExists(name) or
                entity_manager_ptr_->spawnEntity<entity::EgoEntity>(
-                 name, pose, parameters, configuration, node_parameters_);
+                 name, pose, parameters, getCurrentTime(), configuration, node_parameters_);
       } else {
         return entity_manager_ptr_->spawnEntity<entity::VehicleEntity>(
-          name, pose, parameters, behavior);
+          name, pose, parameters, getCurrentTime(), behavior);
       }
     };
 
@@ -184,7 +184,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 = [&]() {
@@ -213,7 +213,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 = [&]() {

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
@@ -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 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/...tion/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp b/...tion/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/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp
@@ -86,7 +86,7 @@ class EgoEntity : public VehicleEntity
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(double current_time, 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
@@ -128,9 +128,9 @@ class EntityBase
 
   virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;
 
-  virtual void onUpdate(double current_time, double step_time);
+  virtual auto onUpdate(const double current_time, const double step_time) -> void;
 
-  virtual void onPostUpdate(double current_time, 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/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -85,10 +85,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 =
@@ -158,7 +154,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(),
@@ -302,29 +297,16 @@ class EntityManager
   FORWARD_TO_ENTITY(setMapPose, );
   FORWARD_TO_ENTITY(setTwist, );
   FORWARD_TO_ENTITY(setVelocityLimit, );
+  FORWARD_TO_ENTITY(requestSpeedChange, );
 
 #undef FORWARD_TO_ENTITY
 
   visualization_msgs::msg::MarkerArray makeDebugMarker() const;
 
   bool trafficLightsChanged();
 
-  void requestSpeedChange(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);
-
-  void requestSpeedChange(
-    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 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();
 
@@ -338,8 +320,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
@@ -357,8 +337,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 &;
 
@@ -417,7 +395,8 @@ class EntityManager
 
   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)
   {
     static_assert(
       std::disjunction<
@@ -446,7 +425,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();
@@ -493,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/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
@@ -68,7 +68,7 @@ class PedestrianEntity : public EntityBase
 
   auto getEntityTypename() const -> const std::string & override;
 
-  void onUpdate(double current_time, 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
@@ -94,7 +94,7 @@ class VehicleEntity : public EntityBase
 
   auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override;
 
-  void onUpdate(double current_time, 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/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,
   double matching_distance, 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
@@ -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)
 {
 }

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -134,7 +134,7 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr
   return field_operator_application->getWaypoints();
 }
 
-void EgoEntity::onUpdate(double current_time, double step_time)
+auto EgoEntity::onUpdate(const double current_time, const double step_time) -> void
 {
   EntityBase::onUpdate(current_time, step_time);
 
@@ -266,15 +266,22 @@ auto EgoEntity::setBehaviorParameter(
   behavior_parameter_ = behavior_parameter;
 }
 
-void EgoEntity::requestSpeedChange(double value, bool)
+auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void
 {
-  target_speed_ = value;
-  field_operator_application->restrictTargetSpeed(value);
+  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);
+  }
 }
 
-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 "
+    "purposes only.");
 }
 
 auto EgoEntity::setVelocityLimit(double value) -> void  //

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -101,7 +101,7 @@ auto EntityBase::isTargetSpeedReached(const speed_change::RelativeTargetSpeed &
     target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_));
 }
 
-void EntityBase::onUpdate(double /*current_time*/, double step_time)
+auto EntityBase::onUpdate(const double /*current_time*/, const double step_time) -> void
 {
   job_list_.update(step_time, job::Event::PRE_UPDATE);
   step_time_ = step_time;
@@ -111,7 +111,7 @@ void EntityBase::onUpdate(double /*current_time*/, double step_time)
       step_time, name);
 }
 
-void EntityBase::onPostUpdate(double /*current_time*/, double step_time)
+auto EntityBase::onPostUpdate(const double /*current_time*/, const double step_time) -> void
 {
   job_list_.update(step_time, job::Event::POST_UPDATE);
 }