fix(obstacle_velocity_limiter): more stable virtual wall (autowarefou…

…ndation#8499)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -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

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -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
@@ -124,15 +115,6 @@ std::vector<autoware::motion_velocity_planner::SlowdownInterval> calculate_slowd
   ProjectionParameters & projection_params, const VelocityParameters & velocity_params,
   autoware::motion_utils::VirtualWalls & virtual_walls);
 
-/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
-/// @param[in] downsampled_trajectory downsampled trajectory
-/// @param[in] trajectory input trajectory
-/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
-/// @param[in] factor downsampling factor
-/// @return input trajectory with the velocity profile of the downsampled trajectory
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // OBSTACLE_VELOCITY_LIMITER_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,14 +15,14 @@
 #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"
 #include "trajectory_preprocessing.hpp"
 
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
@@ -31,7 +31,6 @@
 
 #include <boost/geometry.hpp>
 
-#include <algorithm>
 #include <chrono>
 #include <map>
 
@@ -160,6 +159,9 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
     preprocessing_params_.max_duration);
   auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory(
     original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor);
+  if (prev_inserted_point_) {
+    obstacle_velocity_limiter::add_trajectory_point(downsampled_traj_points, *prev_inserted_point_);
+  }
   obstacle_velocity_limiter::ObstacleMasks obstacle_masks;
   const auto preprocessing_us = stopwatch.toc("preprocessing");
   stopwatch.tic("obstacles");
@@ -203,6 +205,9 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
   }
   virtual_wall_marker_creator.add_virtual_walls(virtual_walls);
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
+  if (!result.slowdown_intervals.empty()) {
+    prev_inserted_point_ = result.slowdown_intervals.front().from;
+  }
 
   const auto total_us = stopwatch.toc();
   RCLCPP_DEBUG(

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,9 +15,7 @@
 #ifndef OBSTACLE_VELOCITY_LIMITER_MODULE_HPP_
 #define OBSTACLE_VELOCITY_LIMITER_MODULE_HPP_
 
-#include "obstacles.hpp"
 #include "parameters.hpp"
-#include "types.hpp"
 
 #include <autoware/motion_velocity_planner_common/plugin_module_interface.hpp>
 #include <autoware/motion_velocity_planner_common/velocity_planning_result.hpp>
@@ -29,6 +27,7 @@
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -38,8 +37,6 @@
 #include <lanelet2_core/LaneletMap.h>
 
 #include <memory>
-#include <optional>
-#include <set>
 #include <string>
 #include <vector>
 
@@ -59,8 +56,8 @@ class ObstacleVelocityLimiterModule : public PluginModuleInterface
 private:
   inline static const std::string ns_ = "obstacle_velocity_limiter";
   std::string module_name_;
-  rclcpp::Clock::SharedPtr clock_{};
-  rclcpp::Time prev_inserted_point_time_{};
+  rclcpp::Clock::SharedPtr clock_;
+  std::optional<geometry_msgs::msg::Point> prev_inserted_point_;
 
   // parameters
   obstacle_velocity_limiter::PreprocessingParameters preprocessing_params_;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/parameters.hpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,6 +22,7 @@
 
 #include <tf2/utils.h>
 
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -45,7 +46,7 @@ struct ObstacleParameters
   int8_t occupancy_grid_threshold{};
   double dynamic_obstacles_buffer{};
   double dynamic_obstacles_min_vel{};
-  std::vector<std::string> static_map_tags{};
+  std::vector<std::string> static_map_tags;
   bool filter_envelope{};
   bool ignore_on_path{};
   double ignore_extra_distance{};
@@ -125,7 +126,7 @@ struct ProjectionParameters
   double velocity{};
   double heading{};
   // parameters specific to the bicycle model
-  int points_per_projection = 5;
+  int64_t points_per_projection = 5;
   double wheel_base{};
   double steering_angle{};
   double steering_angle_offset{};
@@ -172,11 +173,11 @@ struct ProjectionParameters
     return true;
   }
 
-  bool updateNbPoints(const rclcpp::Logger & logger, const int nb_points)
+  bool updateNbPoints(const rclcpp::Logger & logger, const int64_t nb_points)
   {
     if (nb_points < 2) {
       RCLCPP_WARN(
-        logger, "Cannot use less than 2 points per projection. Using value %d instead.",
+        logger, "Cannot use less than 2 points per projection. Using value %ld instead.",
         points_per_projection);
       return false;
     }
@@ -217,7 +218,7 @@ struct PreprocessingParameters
   static constexpr auto MAX_LENGTH_PARAM = "trajectory_preprocessing.max_length";
   static constexpr auto MAX_DURATION_PARAM = "trajectory_preprocessing.max_duration";
 
-  int downsample_factor{};
+  int64_t downsample_factor{};
   double start_distance{};
   bool calculate_steering_angles{};
   double max_length{};
@@ -232,7 +233,7 @@ struct PreprocessingParameters
     max_length = node.declare_parameter<double>(MAX_LENGTH_PARAM);
     max_duration = node.declare_parameter<double>(MAX_DURATION_PARAM);
   }
-  bool updateDownsampleFactor(const int new_downsample_factor)
+  bool updateDownsampleFactor(const int64_t new_downsample_factor)
   {
     if (new_downsample_factor > 0) {
       downsample_factor = new_downsample_factor;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -14,13 +14,17 @@
 
 #include "trajectory_preprocessing.hpp"
 
+#include "types.hpp"
+
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <geometry_msgs/msg/detail/point__struct.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <tf2/utils.h>
 
-#include <algorithm>
+#include <optional>
 
 namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
@@ -57,7 +61,7 @@ size_t calculateEndIndex(
 
 TrajectoryPoints downsampleTrajectory(
   const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
-  const int factor)
+  const int64_t factor)
 {
   if (factor < 1) return trajectory;
   TrajectoryPoints downsampled_traj;
@@ -79,19 +83,24 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
     const auto d_heading = heading - prev_heading;
     prev_heading = heading;
     point.front_wheel_angle_rad =
-      std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt);
+      static_cast<float>(std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt));
   }
 }
 
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor)
+void add_trajectory_point(TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & point)
 {
-  const auto size = std::min(downsampled_traj.size(), trajectory.size());
-  for (size_t i = 0; i < size; ++i) {
-    trajectory[start_idx + i * factor].longitudinal_velocity_mps =
-      downsampled_traj[i].longitudinal_velocity_mps;
+  const auto segment_idx = motion_utils::findNearestSegmentIndex(trajectory, point);
+  const auto lat_offset = motion_utils::calcLateralOffset(trajectory, point, segment_idx);
+  if (lat_offset < 1.0) {
+    const auto lon_offset_to_segment =
+      motion_utils::calcLongitudinalOffsetToSegment(trajectory, segment_idx, point);
+    const auto inserted_idx =
+      motion_utils::insertTargetPoint(segment_idx, lon_offset_to_segment, trajectory);
+    if (inserted_idx) {
+      trajectory[*inserted_idx].longitudinal_velocity_mps =
+        trajectory[segment_idx].longitudinal_velocity_mps;
+      trajectory[*inserted_idx].acceleration_mps2 = trajectory[segment_idx].acceleration_mps2;
+    }
   }
-  return trajectory;
 }
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp

@@ -1,4 +1,4 @@
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -45,23 +45,18 @@ size_t calculateEndIndex(
 /// @return downsampled trajectory
 TrajectoryPoints downsampleTrajectory(
   const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
-  const int factor);
+  const int64_t factor);
 
 /// @brief recalculate the steering angle of the trajectory
 /// @details uses the change in headings for calculation
 /// @param[inout] trajectory input trajectory
 /// @param[in] wheel_base wheel base of the vehicle
 void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base);
 
-/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
-/// @param[in] downsampled_trajectory downsampled trajectory
-/// @param[in] trajectory input trajectory
-/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
-/// @param[in] factor downsampling factor
-/// @return input trajectory with the velocity profile of the downsampled trajectory
-TrajectoryPoints copyDownsampledVelocity(
-  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
-  const int factor);
+/// @brief insert a point in the trajectory and fill its velocity and acceleration data
+/// @param[inout] trajectory input trajectory
+/// @param[in] point point to insert in the trajectory
+void add_trajectory_point(TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & point);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // TRAJECTORY_PREPROCESSING_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -332,9 +332,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)
+    for (auto idx = *from_insert_idx; idx <= *to_insert_idx; ++idx) {
       trajectory.points[idx].longitudinal_velocity_mps =
-        static_cast<float>(slowdown_interval.velocity);
+        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");
   }