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"
/>
+
+
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 @@
autoware_auto_perception_msgs
autoware_auto_planning_msgs
behavior_path_planner
+ behavior_velocity_planner
geometry_msgs
global_parameter_loader
interpolation
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 StaticCenterlineOptimizerNode::plan_path(
const double behavior_path_interval = has_parameter("output_path_interval")
? get_parameter("output_path_interval").as_double()
: declare_parameter("output_path_interval");
+ const double behavior_vel_interval =
+ has_parameter("behavior_output_path_interval")
+ ? get_parameter("behavior_output_path_interval").as_double()
+ : declare_parameter("behavior_output_path_interval");
// extract path with lane id from lanelets
const auto raw_path_with_lane_id = [&]() {
@@ -439,8 +443,7 @@ std::vector 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..36bdfc732ed79 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.path.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",