fix(toLaneletPose): fix matching_distance in EgoEntity, EgoEntitySimu…

…lation and BehaviorTree
tier4 · Apr 5, 2024 · 4d92606 · 4d92606
1 parent 62b9867
commit 4d92606
Show file tree

Hide file tree

Showing 7 changed files with 37 additions and 16 deletions.
diff --git a/...ee_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/...ee_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -64,6 +64,10 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     }
   };
 
+  auto getMatchingDistance = [&]() -> double {
+    return entity_status->getBoundingBox().dimensions.y * 0.5 + 1.0;
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -76,7 +80,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
+      behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

diff --git a/..._tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/..._tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp
@@ -64,6 +64,14 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
     }
   };
 
+  auto getMatchingDistance = [&]() -> double {
+    return std::max(
+             vehicle_parameters.axles.front_axle.track_width,
+             vehicle_parameters.axles.rear_axle.track_width) *
+             0.5 +
+           1.0;
+  };
+
   if (getBlackBoardValues();
       request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or
       not getInput<decltype(polyline_trajectory)>("polyline_trajectory", polyline_trajectory) or
@@ -76,7 +84,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*entity_status), *polyline_trajectory,
-      behavior_parameter, hdmap_utils, step_time, getTargetSpeed())) {
+      behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) {
     setOutput(
       "updated_status",
       std::make_shared<traffic_simulator::CanonicalizedEntityStatus>(

diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp
@@ -324,19 +324,23 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus(
   /// EgoEntity::setMapPose
   const auto unique_route_lanelets =
     traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
-  const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }();
+  const auto matching_distance = std::max(
+                                   vehicle_parameters.axles.front_axle.track_width,
+                                   vehicle_parameters.axles.rear_axle.track_width) *
+                                   0.5 +
+                                 1.0;
 
   std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
 
   if (unique_route_lanelets.empty()) {
     lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+      hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_distance);
   } else {
     lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, matching_length);
+      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, matching_distance);
     if (!lanelet_pose) {
       lanelet_pose =
-        hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+        hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_distance);
     }
   }
   return lanelet_pose;

diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp
@@ -29,7 +29,7 @@ auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus &,
   traffic_simulator_msgs::msg::PolylineTrajectory &,
   const traffic_simulator_msgs::msg::BehaviorParameter &,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double, double,
   std::optional<double> target_speed = std::nullopt)
   -> std::optional<traffic_simulator_msgs::msg::EntityStatus>;
 }  // namespace follow_trajectory

diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp
@@ -47,7 +47,8 @@ auto makeUpdatedStatus(
   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,
-  std::optional<double> target_speed) -> std::optional<traffic_simulator_msgs::msg::EntityStatus>
+  double matching_distance, std::optional<double> target_speed)
+  -> std::optional<traffic_simulator_msgs::msg::EntityStatus>
 {
   using math::arithmetic::isApproximatelyEqualTo;
   using math::arithmetic::isDefinitelyLessThan;
@@ -97,7 +98,8 @@ auto makeUpdatedStatus(
     }
 
     return makeUpdatedStatus(
-      entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time, target_speed);
+      entity_status, polyline_trajectory, behavior_parameter, hdmap_utils, step_time,
+      matching_distance, target_speed);
   };
 
   auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); };
@@ -557,9 +559,8 @@ auto makeUpdatedStatus(
 
     updated_status.time = entity_status.time + step_time;
 
-    // matching distance has been set to 3.0 due to matching problems during lane changes
-    if (const auto lanelet_pose =
-          hdmap_utils->toLaneletPose(updated_status.pose, entity_status.bounding_box, false, 3.0);
+    if (const auto lanelet_pose = hdmap_utils->toLaneletPose(
+          updated_status.pose, entity_status.bounding_box, false, matching_distance);
         lanelet_pose) {
       updated_status.lanelet_pose = lanelet_pose.value();
       updated_status.lanelet_pose_valid = true;

diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp
@@ -156,6 +156,7 @@ void EgoEntity::onUpdate(double current_time, double step_time)
       const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus(
         static_cast<traffic_simulator::EntityStatus>(status_), *polyline_trajectory_,
         behavior_parameter_, hdmap_utils_ptr_, step_time,
+        getDefaultMatchingDistanceForLaneletPoseCalculation(),
         target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) {
       setStatus(CanonicalizedEntityStatus(*updated_status, hdmap_utils_ptr_));
     } else {

diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp
@@ -219,13 +219,16 @@ auto EntityBase::fillLaneletPose(CanonicalizedEntityStatus & status, bool includ
 
   if (unique_route_lanelets.empty()) {
     lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
-      status_non_canonicalized.pose, getBoundingBox(), include_crosswalk, 1.0);
+      status_non_canonicalized.pose, getBoundingBox(), include_crosswalk,
+      getDefaultMatchingDistanceForLaneletPoseCalculation());
   } else {
-    lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status_non_canonicalized.pose, unique_route_lanelets, 1.0);
+    lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
+      status_non_canonicalized.pose, unique_route_lanelets,
+      getDefaultMatchingDistanceForLaneletPoseCalculation());
     if (!lanelet_pose) {
       lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
-        status_non_canonicalized.pose, getBoundingBox(), include_crosswalk, 1.0);
+        status_non_canonicalized.pose, getBoundingBox(), include_crosswalk,
+        getDefaultMatchingDistanceForLaneletPoseCalculation());
     }
   }
   if (lanelet_pose) {