Merge pull request #1184 from tier4/feature/consider_tread_in_ego_entity

Feature/consider tread in ego entity

docs/ReleaseNotes.md

@@ -4,8 +4,9 @@
 
 Major Changes :race_car: :red_car: :blue_car:
 
-| Feature | Brief summary | Category | Pull request | Contributor |
-| ------- | ------------- | -------- | ------------ | ----------- |
+| Feature                 | Brief summary                                                        | Category            | Pull request                                                      | Contributor                                   |
+| ----------------------- | -------------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- |
+| Extend matching length. | Enable consider tread when calculating matching length of EgoEntity. | `traffic_simulator` | [#1181](https://github.com/tier4/scenario_simulator_v2/pull/1181) | [hakuturu583](https://github.com/hakuturu583) |
 
 Bug Fixes:bug:
 

simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp

@@ -92,7 +92,7 @@ class SensorSimulation
   }
 
   auto attachPseudoTrafficLightsDetector(
-    const double current_simulation_time,
+    const double /*current_simulation_time*/,
     const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration,
     rclcpp::Node & node, std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils) -> void
   {

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -62,6 +62,7 @@ class EgoEntitySimulation
 
 public:
   const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
+  const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters;
 
 private:
   auto getCurrentPose() const -> geometry_msgs::msg::Pose;

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -39,7 +39,8 @@ EgoEntitySimulation::EgoEntitySimulation(
 : autoware(std::make_unique<concealer::AutowareUniverse>()),
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
-  hdmap_utils_ptr_(hdmap_utils)
+  hdmap_utils_ptr_(hdmap_utils),
+  vehicle_parameters(parameters)
 {
   autoware->set_parameter(use_sim_time);
 }
@@ -397,12 +398,23 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet(
     traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
   std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
 
+  const auto get_matching_length = [&] {
+    return std::max(
+             vehicle_parameters.axles.front_axle.track_width,
+             vehicle_parameters.axles.rear_axle.track_width) *
+             0.5 +
+           1.0;
+  };
+
   if (unique_route_lanelets.empty()) {
-    lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0);
+    lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
+      status.pose, status.bounding_box, false, get_matching_length());
   } else {
-    lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, 1.0);
+    lanelet_pose =
+      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, get_matching_length());
     if (!lanelet_pose) {
-      lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0);
+      lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
+        status.pose, status.bounding_box, false, get_matching_length());
     }
   }
   if (lanelet_pose) {

simulation/traffic_simulator/include/traffic_simulator/api/api.hpp

@@ -348,13 +348,16 @@ class API
   FORWARD_TO_ENTITY_MANAGER(requestWalkStraight);
   FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate);
   FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate);
+  FORWARD_TO_ENTITY_MANAGER(setAcceleration);
   FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit);
   FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit);
   FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter);
   FORWARD_TO_ENTITY_MANAGER(setConventionalTrafficLightConfidence);
   FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit);
   FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit);
   FORWARD_TO_ENTITY_MANAGER(setLinearVelocity);
+  FORWARD_TO_ENTITY_MANAGER(setMapPose);
+  FORWARD_TO_ENTITY_MANAGER(setTwist);
   FORWARD_TO_ENTITY_MANAGER(setVelocityLimit);
   FORWARD_TO_ENTITY_MANAGER(toMapPose);
 

simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp

@@ -39,8 +39,6 @@ class EgoEntity : public VehicleEntity
   static auto makeFieldOperatorApplication(const Configuration &)
     -> std::unique_ptr<concealer::FieldOperatorApplication>;
 
-  CanonicalizedEntityStatus externally_updated_status_;
-
 public:
   explicit EgoEntity() = delete;
 
@@ -107,15 +105,15 @@ class EgoEntity : public VehicleEntity
   auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &)
     -> void override;
 
-  auto setStatusExternally(const CanonicalizedEntityStatus & status) -> void;
-
   void requestSpeedChange(double, bool continuous) override;
 
   void requestSpeedChange(
     const speed_change::RelativeTargetSpeed & target_speed, bool continuous) override;
 
   auto setVelocityLimit(double) -> void override;
 
+  auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override;
+
   auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override;
 };
 }  // namespace entity

simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp

@@ -219,6 +219,12 @@ class EntityBase
 
   virtual auto setVelocityLimit(double) -> void;
 
+  virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;
+
+  /*   */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void;
+
+  /*   */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void;
+
   virtual void startNpcLogic();
 
   /*   */ void stopAtCurrentPosition();

simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp

@@ -282,7 +282,7 @@ class EntityManager
   // clang-format on
 
   FORWARD_TO_ENTITY(asFieldOperatorApplication, const);
-  FORWARD_TO_ENTITY(cancelRequest, );
+  FORWARD_TO_ENTITY(fillLaneletPose, const);
   FORWARD_TO_ENTITY(get2DPolygon, const);
   FORWARD_TO_ENTITY(getBehaviorParameter, const);
   FORWARD_TO_ENTITY(getBoundingBox, const);
@@ -297,27 +297,30 @@ class EntityManager
   FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const);
   FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const);
   FORWARD_TO_ENTITY(getEntityType, const);
-  FORWARD_TO_ENTITY(fillLaneletPose, const);
   FORWARD_TO_ENTITY(getLaneletPose, const);
   FORWARD_TO_ENTITY(getLinearJerk, const);
   FORWARD_TO_ENTITY(getMapPose, const);
   FORWARD_TO_ENTITY(getMapPoseFromRelativePose, const);
-  FORWARD_TO_ENTITY(getRouteLanelets, );
+  FORWARD_TO_ENTITY(getRouteLanelets, const);
   FORWARD_TO_ENTITY(getStandStillDuration, const);
   FORWARD_TO_ENTITY(getTraveledDistance, const);
   FORWARD_TO_ENTITY(laneMatchingSucceed, const);
+  FORWARD_TO_ENTITY(activateOutOfRangeJob, );
+  FORWARD_TO_ENTITY(cancelRequest, );
   FORWARD_TO_ENTITY(requestAcquirePosition, );
   FORWARD_TO_ENTITY(requestAssignRoute, );
   FORWARD_TO_ENTITY(requestFollowTrajectory, );
   FORWARD_TO_ENTITY(requestLaneChange, );
   FORWARD_TO_ENTITY(requestWalkStraight, );
-  FORWARD_TO_ENTITY(activateOutOfRangeJob, );
+  FORWARD_TO_ENTITY(setAcceleration, );
   FORWARD_TO_ENTITY(setAccelerationLimit, );
   FORWARD_TO_ENTITY(setAccelerationRateLimit, );
   FORWARD_TO_ENTITY(setBehaviorParameter, );
   FORWARD_TO_ENTITY(setDecelerationLimit, );
   FORWARD_TO_ENTITY(setDecelerationRateLimit, );
   FORWARD_TO_ENTITY(setLinearVelocity, );
+  FORWARD_TO_ENTITY(setMapPose, );
+  FORWARD_TO_ENTITY(setTwist, );
   FORWARD_TO_ENTITY(setVelocityLimit, );
 
 #undef FORWARD_TO_ENTITY
@@ -473,9 +476,6 @@ class EntityManager
 
   auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void;
 
-  auto setEntityStatusExternally(const std::string & name, const CanonicalizedEntityStatus &)
-    -> void;
-
   void setVerbose(const bool verbose);
 
   template <typename Entity, typename Pose, typename Parameters, typename... Ts>

simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp

@@ -119,6 +119,8 @@ class PedestrianEntity : public EntityBase
 
   const std::string plugin_name;
 
+  const traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters;
+
 private:
   pluginlib::ClassLoader<entity_behavior::BehaviorPluginBase> loader_;
   const std::shared_ptr<entity_behavior::BehaviorPluginBase> behavior_plugin_ptr_;

simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp

@@ -122,6 +122,8 @@ class VehicleEntity : public EntityBase
 
   auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override;
 
+  const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters;
+
 private:
   pluginlib::ClassLoader<entity_behavior::BehaviorPluginBase> loader_;
 

simulation/traffic_simulator/src/api/api.cpp

@@ -237,13 +237,9 @@ bool API::updateEntitiesStatusInSim()
       simulation_interface::toMsg(res_status.action_status(), entity_status.action_status);
 
       if (entity_manager_ptr_->isEgo(name)) {
-        // temporarily deinitialize lanelet pose as it should be correctly filled from here
-        entity_status.lanelet_pose_valid = false;
-        entity_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose();
-        auto canonicalized = canonicalize(entity_status);
-        /// @note apply additional status data (from ll2) and then set status
-        entity_manager_ptr_->fillLaneletPose(name, canonicalized);
-        entity_manager_ptr_->setEntityStatusExternally(name, canonicalized);
+        setMapPose(name, entity_status.pose);
+        setTwist(name, entity_status.action_status.twist);
+        setAcceleration(name, entity_status.action_status.accel);
       } else {
         setEntityStatus(name, canonicalize(entity_status));
       }

simulation/traffic_simulator/src/entity/ego_entity.cpp

@@ -82,8 +82,7 @@ EgoEntity::EgoEntity(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters,
   const Configuration & configuration)
 : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters),
-  field_operator_application(makeFieldOperatorApplication(configuration)),
-  externally_updated_status_(entity_status)
+  field_operator_application(makeFieldOperatorApplication(configuration))
 {
 }
 
@@ -155,7 +154,6 @@ 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);
@@ -253,11 +251,6 @@ auto EgoEntity::setBehaviorParameter(const traffic_simulator_msgs::msg::Behavior
 {
 }
 
-auto EgoEntity::setStatusExternally(const CanonicalizedEntityStatus & status) -> void
-{
-  externally_updated_status_ = status;
-}
-
 void EgoEntity::requestSpeedChange(double value, bool)
 {
   field_operator_application->restrictTargetSpeed(value);
@@ -278,5 +271,45 @@ auto EgoEntity::fillLaneletPose(CanonicalizedEntityStatus & status) -> void
   EntityBase::fillLaneletPose(status, false);
 }
 
+auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void
+{
+  const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets());
+  std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
+  const auto get_matching_length = [&] {
+    return std::max(
+             vehicle_parameters.axles.front_axle.track_width,
+             vehicle_parameters.axles.rear_axle.track_width) *
+             0.5 +
+           1.0;
+  };
+  if (unique_route_lanelets.empty()) {
+    lanelet_pose =
+      hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length());
+  } else {
+    lanelet_pose =
+      hdmap_utils_ptr_->toLaneletPose(map_pose, unique_route_lanelets, get_matching_length());
+    if (!lanelet_pose) {
+      lanelet_pose =
+        hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length());
+    }
+  }
+  geometry_msgs::msg::Pose map_pose_z_fixed = map_pose;
+  auto status = static_cast<EntityStatus>(status_);
+  if (lanelet_pose) {
+    math::geometry::CatmullRomSpline spline(
+      hdmap_utils_ptr_->getCenterPoints(lanelet_pose->lanelet_id));
+    if (const auto s_value = spline.getSValue(map_pose)) {
+      map_pose_z_fixed.position.z = spline.getPoint(s_value.value()).z;
+    }
+    status.pose = map_pose_z_fixed;
+    status.lanelet_pose_valid = true;
+    status.lanelet_pose = lanelet_pose.value();
+  } else {
+    status.pose = map_pose;
+    status.lanelet_pose_valid = false;
+    status.lanelet_pose = LaneletPose();
+  }
+  status_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_);
+}
 }  // namespace entity
 }  // namespace traffic_simulator

simulation/traffic_simulator/src/entity/entity_base.cpp

@@ -738,6 +738,26 @@ void EntityBase::setTrafficLightManager(
   traffic_light_manager_ = traffic_light_manager;
 }
 
+auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void
+{
+  auto new_status = static_cast<EntityStatus>(getStatus());
+  new_status.action_status.twist = twist;
+  status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
+}
+
+auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void
+{
+  auto new_status = static_cast<EntityStatus>(getStatus());
+  new_status.action_status.accel = accel;
+  status_ = CanonicalizedEntityStatus(new_status, hdmap_utils_ptr_);
+}
+
+auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void
+{
+  THROW_SEMANTIC_ERROR(
+    "You cannot set map pose to the vehicle other than ego named ", std::quoted(name), ".");
+}
+
 void EntityBase::activateOutOfRangeJob(
   double min_velocity, double max_velocity, double min_acceleration, double max_acceleration,
   double min_jerk, double max_jerk)

simulation/traffic_simulator/src/entity/entity_manager.cpp

@@ -711,18 +711,6 @@ auto EntityManager::setEntityStatus(
   }
 }
 
-auto EntityManager::setEntityStatusExternally(
-  const std::string & name, const CanonicalizedEntityStatus & status) -> void
-{
-  if (not isEgo(name)) {
-    THROW_SEMANTIC_ERROR(
-      "You cannot set entity status externally to the vehicle other than ego named ",
-      std::quoted(name), ".");
-  } else {
-    dynamic_cast<EgoEntity *>(entities_[name].get())->setStatusExternally(status);
-  }
-}
-
 void EntityManager::setVerbose(const bool verbose)
 {
   configuration.verbose = verbose;

simulation/traffic_simulator/src/entity/pedestrian_entity.cpp

@@ -31,6 +31,7 @@ PedestrianEntity::PedestrianEntity(
   const std::string & plugin_name)
 : EntityBase(name, entity_status, hdmap_utils_ptr),
   plugin_name(plugin_name),
+  pedestrian_parameters(parameters),
   loader_(pluginlib::ClassLoader<entity_behavior::BehaviorPluginBase>(
     "traffic_simulator", "entity_behavior::BehaviorPluginBase")),
   behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)),

simulation/traffic_simulator/src/entity/vehicle_entity.cpp

@@ -31,6 +31,7 @@ VehicleEntity::VehicleEntity(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters,
   const std::string & plugin_name)
 : EntityBase(name, entity_status, hdmap_utils_ptr),
+  vehicle_parameters(parameters),
   loader_(pluginlib::ClassLoader<entity_behavior::BehaviorPluginBase>(
     "traffic_simulator", "entity_behavior::BehaviorPluginBase")),
   behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)),