feat(occlusion spot): consider object velocity direction (autowarefou…

…ndation#4650) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Aug 17, 2023 · 420af00 · 420af00
1 parent 2da561f
commit 420af00
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp
@@ -186,15 +186,19 @@ bool isVehicle(const PredictedObject & obj)
 bool isStuckVehicle(const PredictedObject & obj, const double min_vel)
 {
   if (!isVehicle(obj)) return false;
-  const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x;
-  return std::abs(obj_vel) <= min_vel;
+  const auto & obj_vel_norm = std::hypot(
+    obj.kinematics.initial_twist_with_covariance.twist.linear.x,
+    obj.kinematics.initial_twist_with_covariance.twist.linear.y);
+  return obj_vel_norm <= min_vel;
 }
 
 bool isMovingVehicle(const PredictedObject & obj, const double min_vel)
 {
   if (!isVehicle(obj)) return false;
-  const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x;
-  return std::abs(obj_vel) > min_vel;
+  const auto & obj_vel_norm = std::hypot(
+    obj.kinematics.initial_twist_with_covariance.twist.linear.x,
+    obj.kinematics.initial_twist_with_covariance.twist.linear.y);
+  return obj_vel_norm > min_vel;
 }
 
 std::vector<PredictedObject> extractVehicles(