feat(ego_entity): update BT, overwrite only FollowTrajectoryAction

tier4 · Jan 25, 2024 · e1286d6 · e1286d6
1 parent bb169ba
commit e1286d6
Show file tree

Hide file tree

Showing 10 changed files with 32 additions and 9 deletions.
diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto
@@ -188,6 +188,7 @@ message DespawnEntityResponse {
 message UpdateEntityStatusRequest {
   repeated EntityStatus status = 1;        // List of updated entity status in traffic simulator.
   bool npc_logic_started = 2;              // Npc logic started flag
+  bool overwrite_ego_status = 3;
 }
 
 /**

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
@@ -182,6 +182,8 @@ class EntityBase
 
   virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool);
 
+  virtual auto isFollowTrajectoryActionRun() const -> bool;
+
   virtual auto requestFollowTrajectory(
     const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void;
 

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp
@@ -309,6 +309,7 @@ class EntityManager
   FORWARD_TO_ENTITY(laneMatchingSucceed, const);
   FORWARD_TO_ENTITY(requestAcquirePosition, );
   FORWARD_TO_ENTITY(requestAssignRoute, );
+  FORWARD_TO_ENTITY(isFollowTrajectoryActionRun, );
   FORWARD_TO_ENTITY(requestFollowTrajectory, );
   FORWARD_TO_ENTITY(requestLaneChange, );
   FORWARD_TO_ENTITY(requestWalkStraight, );

diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp
@@ -99,6 +99,8 @@ class PedestrianEntity : public EntityBase
 
   void requestAssignRoute(const std::vector<geometry_msgs::msg::Pose> &) override;
 
+  auto isFollowTrajectoryActionRun() const -> bool override;
+
   auto requestFollowTrajectory(
     const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void 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
@@ -100,6 +100,8 @@ class VehicleEntity : public EntityBase
 
   void requestAssignRoute(const std::vector<CanonicalizedLaneletPose> &) override;
 
+  auto isFollowTrajectoryActionRun() const -> bool override;
+
   auto requestFollowTrajectory(
     const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void override;
 

diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp
@@ -226,6 +226,9 @@ bool API::updateEntitiesStatusInSim()
   for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) {
     auto entity_status = entity_manager_ptr_->getEntityStatus(entity_name);
     simulation_interface::toProto(static_cast<EntityStatus>(entity_status), *req.add_status());
+    if (entity_manager_ptr_->isEgo(entity_name)) {
+      req.set_overwrite_ego_status(entity_manager_ptr_->isFollowTrajectoryActionRun(entity_name));
+    }
   }
 
   simulation_api_schema::UpdateEntityStatusResponse res;

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -96,7 +96,8 @@ auto EgoEntity::asFieldOperatorApplication() const -> concealer::FieldOperatorAp
 auto EgoEntity::getCurrentAction() const -> std::string
 {
   const auto state = field_operator_application->getAutowareStateName();
-  return state.empty() ? "Launching" : state;
+  return (state.empty() ? "Launching" : state) +
+         (isFollowTrajectoryActionRun() ? " (" + VehicleEntity::getCurrentAction() + ")" : "");
 }
 
 auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter
@@ -154,16 +155,10 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr
 
 void EgoEntity::onUpdate(double current_time, double step_time)
 {
-  EntityBase::onUpdate(current_time, step_time);
   setStatus(externally_updated_status_);
-
-  updateStandStillDuration(step_time);
-  updateTraveledDistance(step_time);
-
+  VehicleEntity::onUpdate(current_time, step_time);
   field_operator_application->rethrow();
   field_operator_application->spinSome();
-
-  EntityBase::onPostUpdate(current_time, step_time);
 }
 
 void EgoEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose)
@@ -258,8 +253,9 @@ auto EgoEntity::setStatusExternally(const CanonicalizedEntityStatus & status) ->
   externally_updated_status_ = status;
 }
 
-void EgoEntity::requestSpeedChange(double value, bool)
+void EgoEntity::requestSpeedChange(double value, bool continuous)
 {
+  VehicleEntity::requestSpeedChange(value, continuous);
   field_operator_application->restrictTargetSpeed(value);
 }
 

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -649,6 +649,12 @@ void EntityBase::requestSpeedChange(
   }
 }
 
+auto EntityBase::isFollowTrajectoryActionRun() const -> bool
+{
+  THROW_SEMANTIC_ERROR(
+    getEntityTypename(), " type entities do not support follow trajectory action.");
+}
+
 auto EntityBase::requestFollowTrajectory(
   const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void
 {

diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp
@@ -77,6 +77,11 @@ void PedestrianEntity::requestAssignRoute(const std::vector<geometry_msgs::msg::
   requestAssignRoute(route);
 }
 
+auto PedestrianEntity::isFollowTrajectoryActionRun() const -> bool
+{
+  return behavior_plugin_ptr_->getCurrentAction() == "follow_polyline_trajectory";
+}
+
 auto PedestrianEntity::requestFollowTrajectory(
   const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & parameter) -> void
 {

diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp
@@ -222,6 +222,11 @@ void VehicleEntity::requestAssignRoute(const std::vector<geometry_msgs::msg::Pos
   requestAssignRoute(route);
 }
 
+auto VehicleEntity::isFollowTrajectoryActionRun() const -> bool
+{
+  return behavior_plugin_ptr_->getCurrentAction() == "follow_polyline_trajectory";
+}
+
 auto VehicleEntity::requestFollowTrajectory(
   const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & parameter) -> void
 {