From 814cd8f39f4462c120e8892911f99113ead173d3 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Mon, 15 Jan 2024 02:59:39 +0900
Subject: [PATCH] feat(static_centerline_optimizer): get
 behavior_velocity_planner's path interval from yaml

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../launch/static_centerline_optimizer.launch.xml          | 5 +++++
 planning/static_centerline_optimizer/package.xml           | 1 +
 .../src/static_centerline_optimizer_node.cpp               | 7 +++++--
 .../test/test_static_centerline_optimizer.test.py          | 4 ++++
 4 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml
index f8b96ca0db146..37a9abc47bfeb 100644
--- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml
+++ b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml
@@ -21,6 +21,10 @@
     name="behavior_path_planner_param"
     default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml"
   />
+  <arg
+    name="behavior_velocity_planner_param"
+    default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"
+  />
   <arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
   <arg
     name="obstacle_avoidance_planner_param"
@@ -67,6 +71,7 @@
     <!-- component param -->
     <param from="$(var map_loader_param)"/>
     <param from="$(var behavior_path_planner_param)"/>
+    <param from="$(var behavior_velocity_planner_param)"/>
     <param from="$(var path_smoother_param)"/>
     <param from="$(var obstacle_avoidance_planner_param)"/>
     <param from="$(var mission_planner_param)"/>
diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml
index 51bd9e87d6ba2..17191868b7418 100644
--- a/planning/static_centerline_optimizer/package.xml
+++ b/planning/static_centerline_optimizer/package.xml
@@ -19,6 +19,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>behavior_path_planner</depend>
+  <depend>behavior_velocity_planner</depend>
   <depend>geometry_msgs</depend>
   <depend>global_parameter_loader</depend>
   <depend>interpolation</depend>
diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp
index f2d9dc68a8cdc..d98341ecb2e23 100644
--- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp
+++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp
@@ -425,6 +425,10 @@ std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::plan_path(
   const double behavior_path_interval = has_parameter("output_path_interval")
                                           ? get_parameter("output_path_interval").as_double()
                                           : declare_parameter<double>("output_path_interval");
+  const double behavior_vel_interval =
+    has_parameter("behavior_output_path_interval")
+      ? get_parameter("behavior_output_path_interval").as_double()
+      : declare_parameter<double>("behavior_output_path_interval");
 
   // extract path with lane id from lanelets
   const auto raw_path_with_lane_id = [&]() {
@@ -439,8 +443,7 @@ std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::plan_path(
   // convert path with lane id to path
   const auto raw_path = [&]() {
     const auto non_resampled_path = convert_to_path(raw_path_with_lane_id);
-    // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere.
-    return motion_utils::resamplePath(non_resampled_path, 1.0);
+    return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval);
   }();
   pub_raw_path_->publish(raw_path);
   RCLCPP_INFO(get_logger(), "Converted to path and published.");
diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py
index 141743deb007c..e0768372a7f54 100644
--- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py
+++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py
@@ -56,6 +56,10 @@ def generate_test_description():
                 get_package_share_directory("behavior_path_planner"),
                 "config/behavior_path_planner.param.yaml",
             ),
+            os.velocity.join(
+                get_package_share_directory("behavior_velocity_planner"),
+                "config/behavior_velocity_planner.param.yaml",
+            ),
             os.path.join(
                 get_package_share_directory("path_smoother"),
                 "config/elastic_band_smoother.param.yaml",