insert the previous slowdown point in the downsampled trajectory

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.

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.
@@ -115,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.
@@ -22,6 +22,7 @@
 
 #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_lanelet2_extension/utility/message_conversion.hpp>
@@ -159,6 +160,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");
@@ -202,6 +206,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_