diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py
index 2753861269124..8c361474034ad 100644
--- a/common/autoware_testing/autoware_testing/smoke_test.py
+++ b/common/autoware_testing/autoware_testing/smoke_test.py
@@ -16,6 +16,7 @@
 
 import os
 import shlex
+import time
 import unittest
 
 from ament_index_python import get_package_share_directory
@@ -26,21 +27,24 @@
 from launch_ros.actions import Node
 import launch_testing
 import pytest
+import rclpy
 
 
 def resolve_node(context, *args, **kwargs):
+    parameters = [
+        os.path.join(
+            get_package_share_directory(LaunchConfiguration("arg_package").perform(context)),
+            "param",
+            file_name,
+        )
+        for file_name in shlex.split(LaunchConfiguration("arg_param_filenames").perform(context))
+    ]
 
     smoke_test_node = Node(
         package=LaunchConfiguration("arg_package"),
         executable=LaunchConfiguration("arg_package_exe"),
         namespace="test",
-        parameters=[
-            os.path.join(
-                get_package_share_directory(LaunchConfiguration("arg_package").perform(context)),
-                "param",
-                LaunchConfiguration("arg_param_filename").perform(context),
-            )
-        ],
+        parameters=parameters,
         arguments=shlex.split(LaunchConfiguration("arg_executable_arguments").perform(context)),
     )
     return [smoke_test_node]
@@ -55,8 +59,8 @@ def generate_test_description():
     arg_package_exe = DeclareLaunchArgument(
         "arg_package_exe", default_value=["default"], description="Tested executable"
     )
-    arg_param_filename = DeclareLaunchArgument(
-        "arg_param_filename", default_value=["test.param.yaml"], description="Test param file"
+    arg_param_filenames = DeclareLaunchArgument(
+        "arg_param_filenames", default_value=["test.param.yaml"], description="Test param file"
     )
     arg_executable_arguments = DeclareLaunchArgument(
         "arg_executable_arguments", default_value=[""], description="Tested executable arguments"
@@ -66,7 +70,7 @@ def generate_test_description():
         [
             arg_package,
             arg_package_exe,
-            arg_param_filename,
+            arg_param_filenames,
             arg_executable_arguments,
             OpaqueFunction(function=resolve_node),
             launch_testing.actions.ReadyToTest(),
@@ -74,8 +78,19 @@ def generate_test_description():
     )
 
 
+class DummyTest(unittest.TestCase):
+    def test_wait_for_node_ready(self):
+        """Waiting for the node is ready."""
+        rclpy.init()
+        test_node = rclpy.create_node("test_node")
+        while len(test_node.get_node_names()) == 0:
+            time.sleep(0.1)
+            print("waiting for Node to be ready")
+        rclpy.shutdown()
+
+
 @launch_testing.post_shutdown_test()
 class TestProcessOutput(unittest.TestCase):
     def test_exit_code(self, proc_output, proc_info):
-        # Check that process exits with code -15 code: termination request, sent to the program
-        launch_testing.asserts.assertExitCodes(proc_info, [-15])
+        # Check that process exits with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)
diff --git a/common/autoware_testing/cmake/add_smoke_test.cmake b/common/autoware_testing/cmake/add_smoke_test.cmake
index 2998db4960de4..ee2cb0f06e789 100644
--- a/common/autoware_testing/cmake/add_smoke_test.cmake
+++ b/common/autoware_testing/cmake/add_smoke_test.cmake
@@ -19,18 +19,18 @@
 # :type package_name: string
 # :param package_exec: package executable to run during smoke test
 # :type executable_name: string
-# :param PARAM_FILENAME: yaml filename containing test parameters
-# :type PARAM_FILENAME: string
+# :param PARAM_FILENAMES: yaml filenames containing test parameters
+# :type PARAM_FILENAMES: string
 # :param EXECUTABLE_ARGUMENTS: arguments passed to tested executable
 # :type EXECUTABLE_ARGUMENTS: string
 
 function(add_smoke_test package_name executable_name)
-  cmake_parse_arguments(PARSE_ARGV 2 smoke_test "" "PARAM_FILENAME;EXECUTABLE_ARGUMENTS" "")
+  cmake_parse_arguments(PARSE_ARGV 2 smoke_test "" "PARAM_FILENAMES;EXECUTABLE_ARGUMENTS" "")
 
   set(ARGUMENTS "arg_package:=${package_name}" "arg_package_exe:=${executable_name}")
 
-  if(smoke_test_PARAM_FILENAME)
-    list(APPEND ARGUMENTS "arg_param_filename:=${smoke_test_PARAM_FILENAME}")
+  if(smoke_test_PARAM_FILENAMES)
+    list(APPEND ARGUMENTS "arg_param_filenames:=${smoke_test_PARAM_FILENAMES}")
   endif()
 
   if(smoke_test_EXECUTABLE_ARGUMENTS)
diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt
index 7996ac0713107..fa005c1c0bfda 100644
--- a/control/trajectory_follower_nodes/CMakeLists.txt
+++ b/control/trajectory_follower_nodes/CMakeLists.txt
@@ -57,10 +57,16 @@ if(BUILD_TESTING)
   ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node)
   target_link_libraries(${TRAJECTORY_FOLLOWER_NODES_TEST} ${LATLON_MUXER_NODE} ${LATERAL_CONTROLLER_NODE} ${LONGITUDINAL_CONTROLLER_NODE})
 
-  #find_package(autoware_testing REQUIRED)
-  #add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe PARAM_FILENAME latlon_muxer_defaults.param.yaml)
-  #add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe PARAM_FILENAME lateral_controller_defaults.param.yaml)
-  #add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe PARAM_FILENAME longitudinal_controller_defaults.param.yaml)
+  find_package(autoware_testing REQUIRED)
+  add_smoke_test(${PROJECT_NAME} ${LATLON_MUXER_NODE}_exe
+    PARAM_FILENAMES "latlon_muxer_defaults.param.yaml"
+  )
+  add_smoke_test(${PROJECT_NAME} ${LATERAL_CONTROLLER_NODE}_exe
+    PARAM_FILENAMES "lateral_controller_defaults.param.yaml test_vehicle_info.param.yaml"
+  )
+  add_smoke_test(${PROJECT_NAME} ${LONGITUDINAL_CONTROLLER_NODE}_exe
+    PARAM_FILENAMES "longitudinal_controller_defaults.param.yaml test_vehicle_info.param.yaml"
+  )
 endif()
 
 ament_auto_package(
diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
index 2ecf683fd3028..31c6420a67870 100644
--- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
+++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -105,7 +105,7 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   for (size_t i = 0; i < output->size(); ++i) {
     double curvature = 0.0;
     const size_t start = i > after_decel_index ? i - after_decel_index : 0;
-    const size_t end = std::min(output->size(), i + before_decel_index);
+    const size_t end = std::min(output->size(), i + before_decel_index + 1);
     for (size_t j = start; j < end; ++j) {
       curvature = std::max(curvature, std::fabs(curvature_v->at(j)));
     }