enable set route lanelet in DoNothingPlugin

Signed-off-by: Masaya Kataoka <[email protected]>

Author: hakuturu583

simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp

@@ -53,7 +53,6 @@ public:                                         \
   DEFINE_GETTER_SETTER(OtherEntityStatus,                                EntityStatusDict)
   DEFINE_GETTER_SETTER(PedestrianParameters,                             traffic_simulator_msgs::msg::PedestrianParameters)
   DEFINE_GETTER_SETTER(ReferenceTrajectory,                              std::shared_ptr<math::geometry::CatmullRomSpline>)
-  DEFINE_GETTER_SETTER(RouteLanelets,                                    lanelet::Ids)
   DEFINE_GETTER_SETTER(TargetSpeed,                                      std::optional<double>)
   DEFINE_GETTER_SETTER(TrafficLights,                                    std::shared_ptr<traffic_simulator::TrafficLightsBase>)
   DEFINE_GETTER_SETTER(VehicleParameters,                                traffic_simulator_msgs::msg::VehicleParameters)
@@ -77,6 +76,7 @@ private:                                                               \
   DEFINE_GETTER_SETTER(HdMapUtils,                std::shared_ptr<hdmap_utils::HdMapUtils>,                         hdmap_utils_)
   DEFINE_GETTER_SETTER(PolylineTrajectory,        std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory>, polyline_trajectory)
   DEFINE_GETTER_SETTER(Request,                   traffic_simulator::behavior::Request,                             request)
+  DEFINE_GETTER_SETTER(RouteLanelets,             lanelet::Ids,                                                     route_lanelets_)
   DEFINE_GETTER_SETTER(StepTime,                  double,                                                           step_time_)
   // clang-format on
 #undef DEFINE_GETTER_SETTER

simulation/traffic_simulator/src/entity/pedestrian_entity.cpp

@@ -129,6 +129,12 @@ auto PedestrianEntity::getRouteLanelets(double horizon) -> lanelet::Ids
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon);
   } else {
+    if (
+      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
+        status_->getMapPose(), getBoundingBox(), route_planner_.getWholeRouteLanelets(), true,
+        getDefaultMatchingDistanceForLaneletPoseCalculation())) {
+      return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon);
+    }
     return {};
   }
 }
@@ -288,15 +294,6 @@ auto PedestrianEntity::onUpdate(const double current_time, const double step_tim
   /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true
   behavior_plugin_ptr_->update(current_time, step_time);
 
-  /// Try lane matching if the output of behavior plugin is not on lane.
-  if (
-    const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
-      status_->getMapPose(), getBoundingBox(), route_planner_.getWholeRouteLanelets(), true,
-      getDefaultMatchingDistanceForLaneletPoseCalculation())) {
-    status_->set(
-      CanonicalizedEntityStatus(static_cast<EntityStatus>(*status_), canonicalized_lanelet_pose));
-  }
-
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) {
       stopAtCurrentPosition();

simulation/traffic_simulator/src/entity/vehicle_entity.cpp

@@ -116,6 +116,12 @@ auto VehicleEntity::getRouteLanelets(double horizon) -> lanelet::Ids
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon);
   } else {
+    if (
+      const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
+        status_->getMapPose(), getBoundingBox(), route_planner_.getWholeRouteLanelets(), true,
+        getDefaultMatchingDistanceForLaneletPoseCalculation())) {
+      return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon);
+    }
     return {};
   }
 }
@@ -161,15 +167,6 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time)
   /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true
   behavior_plugin_ptr_->update(current_time, step_time);
 
-  /// Try lane matching if the output of behavior plugin is not on lane.
-  if (
-    const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
-      status_->getMapPose(), getBoundingBox(), route_planner_.getWholeRouteLanelets(), true,
-      getDefaultMatchingDistanceForLaneletPoseCalculation())) {
-    status_->set(
-      CanonicalizedEntityStatus(static_cast<EntityStatus>(*status_), canonicalized_lanelet_pose));
-  }
-
   if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) {
     if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) {
       stopAtCurrentPosition();