feat(obstacle_velocity_limiter): consider object velocity direction (#…

…4649) * feat(obstacle_velocity_limiter): consider object velocity direction Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Aug 17, 2023 · 4d66811 · 4d66811
1 parent 420af00
commit 4d66811
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp
@@ -64,9 +64,10 @@ multipolygon_t createObjectPolygons(
 {
   multipolygon_t polygons;
   for (const auto & object : objects.objects) {
-    if (
-      object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_velocity ||
-      object.kinematics.initial_twist_with_covariance.twist.linear.x <= -min_velocity) {
+    const double obj_vel_norm = std::hypot(
+      object.kinematics.initial_twist_with_covariance.twist.linear.x,
+      object.kinematics.initial_twist_with_covariance.twist.linear.y);
+    if (min_velocity <= obj_vel_norm) {
       polygons.push_back(createObjectPolygon(
         object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer));
     }