fix(obstacle_velocity_limiter): only show the 1st virtual wall

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp

@@ -27,15 +27,6 @@
 namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
 
-double calculateSafeVelocity(
-  const TrajectoryPoint & trajectory_point, const double dist_to_collision,
-  const double time_buffer, const double min_adjusted_velocity)
-{
-  return std::min(
-    static_cast<double>(trajectory_point.longitudinal_velocity_mps),
-    std::max(min_adjusted_velocity, dist_to_collision / time_buffer));
-}
-
 multi_polygon_t createPolygonMasks(
   const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer,
   const double min_vel)
@@ -123,36 +114,35 @@ std::vector<autoware::motion_velocity_planner::SlowdownInterval> calculate_slowd
   autoware::motion_utils::VirtualWalls & virtual_walls)
 {
   std::vector<autoware::motion_velocity_planner::SlowdownInterval> slowdown_intervals;
-  double time = 0.0;
+  size_t previous_slowdown_index = trajectory.size();
   for (size_t i = 0; i < trajectory.size(); ++i) {
     auto & trajectory_point = trajectory[i];
-    if (i > 0) {
-      const auto & prev_point = trajectory[i - 1];
-      time += static_cast<double>(
-        autoware::universe_utils::calcDistance2d(prev_point, trajectory_point) /
-        prev_point.longitudinal_velocity_mps);
-    }
     // First linestring is used to calculate distance
     if (projections[i].empty()) continue;
     projection_params.update(trajectory_point);
     const auto dist_to_collision = distanceToClosestCollision(
       projections[i][0], footprints[i], collision_checker, projection_params);
     if (dist_to_collision) {
       const auto min_feasible_velocity =
-        velocity_params.current_ego_velocity - velocity_params.max_deceleration * time;
-
+        velocity_params.current_ego_velocity -
+        velocity_params.max_deceleration *
+          rclcpp::Duration(trajectory_point.time_from_start).seconds();
+
+      const auto safe_velocity = std::max(
+        static_cast<double>(*dist_to_collision - projection_params.extra_length) /
+          static_cast<double>(projection_params.duration),
+        velocity_params.min_velocity);
       slowdown_intervals.emplace_back(
         trajectory_point.pose.position, trajectory_point.pose.position,
-        std::max(
-          min_feasible_velocity,
-          calculateSafeVelocity(
-            trajectory_point,
-            static_cast<double>(*dist_to_collision - projection_params.extra_length),
-            static_cast<double>(projection_params.duration), velocity_params.min_velocity)));
-
-      autoware::motion_utils::VirtualWall wall;
-      wall.pose = trajectory_point.pose;
-      virtual_walls.push_back(wall);
+        std::max(min_feasible_velocity, safe_velocity));
+
+      // with consecutive slowdowns only add a virtual wall for the first one
+      if (previous_slowdown_index + 1 != i) {
+        autoware::motion_utils::VirtualWall wall;
+        wall.pose = trajectory_point.pose;
+        virtual_walls.push_back(wall);
+      }
+      previous_slowdown_index = i;
     }
   }
   return slowdown_intervals;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp

@@ -24,19 +24,10 @@
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
-#include <string>
 #include <vector>
 
 namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
-
-/// @brief calculate the apparent safe velocity
-/// @param[in] trajectory_point trajectory point for which to calculate the apparent safe velocity
-/// @param[in] dist_to_collision distance from the trajectory point to the apparent collision
-/// @return apparent safe velocity
-double calculateSafeVelocity(
-  const TrajectoryPoint & trajectory_point, const double dist_to_collision);
-
 /// @brief calculate trajectory index that is ahead of the given index by the given distance
 /// @param[in] trajectory trajectory
 /// @param[in] ego_idx index closest to the current ego position in the trajectory

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -15,7 +15,6 @@
 #include "obstacle_velocity_limiter_module.hpp"
 
 #include "debug.hpp"
-#include "forward_projection.hpp"
 #include "map_utils.hpp"
 #include "obstacle_velocity_limiter.hpp"
 #include "parameters.hpp"
@@ -32,7 +31,6 @@
 
 #include <boost/geometry.hpp>
 
-#include <algorithm>
 #include <chrono>
 #include <map>
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -134,7 +134,7 @@ bool MotionVelocityPlannerNode::update_planner_data()
   const auto check_with_log = [&](const auto ptr, const auto & log) {
     constexpr auto throttle_duration = 3000;  // [ms]
     if (!ptr) {
-      RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log);
+      RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, "%s", log);
       is_ready = false;
       return false;
     }
@@ -324,8 +324,12 @@ void MotionVelocityPlannerNode::insert_slowdown(
   const auto to_insert_idx =
     autoware::motion_utils::insertTargetPoint(to_seg_idx, slowdown_interval.to, trajectory.points);
   if (from_insert_idx && to_insert_idx) {
-    for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx)
-      trajectory.points[idx].longitudinal_velocity_mps = slowdown_interval.velocity;
+    for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) {
+      trajectory.points[idx].longitudinal_velocity_mps =
+        std::min(  // prevent the node from increasing the velocity
+          trajectory.points[idx].longitudinal_velocity_mps,
+          static_cast<float>(slowdown_interval.velocity));
+    }
   } else {
     RCLCPP_WARN(get_logger(), "Failed to insert slowdown point");
   }