diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
index 75f9e7f7af187..56456b08aac1e 100644
--- a/planning/behavior_path_planner/CMakeLists.txt
+++ b/planning/behavior_path_planner/CMakeLists.txt
@@ -14,14 +14,11 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED
   src/behavior_path_planner_node.cpp
   src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
   src/scene_module/dynamic_avoidance/manager.cpp
-  src/scene_module/side_shift/side_shift_module.cpp
-  src/scene_module/side_shift/manager.cpp
   src/scene_module/lane_change/interface.cpp
   src/scene_module/lane_change/normal.cpp
   src/scene_module/lane_change/external_request.cpp
   src/scene_module/lane_change/manager.cpp
   src/utils/lane_change/utils.cpp
-  src/utils/side_shift/util.cpp
   src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/marker_utils/lane_change/debug.cpp
 )
diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml
index fed2e1c9af3c3..2028b0d571e68 100644
--- a/planning/behavior_path_planner/plugins.xml
+++ b/planning/behavior_path_planner/plugins.xml
@@ -1,5 +1,4 @@
 <library path="behavior_path_planner_lib">
-  <class type="behavior_path_planner::SideShiftModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
   <class type="behavior_path_planner::DynamicAvoidanceModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
   <class type="behavior_path_planner::LaneChangeRightModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
   <class type="behavior_path_planner::LaneChangeLeftModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
index 0e253c0c73985..bab77f9141a00 100644
--- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
+++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
@@ -50,7 +50,6 @@ std::shared_ptr<BehaviorPathPlannerNode> generateNode()
 
   std::vector<std::string> module_names;
   module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager");
-  module_names.emplace_back("behavior_path_planner::SideShiftModuleManager");
   module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager");
   module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager");
   module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager");
@@ -69,8 +68,7 @@ std::shared_ptr<BehaviorPathPlannerNode> generateNode()
      behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
      behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
      behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml",
-     behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml",
-     behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"});
+     behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml"});
 
   return std::make_shared<BehaviorPathPlannerNode>(node_options);
 }
diff --git a/planning/behavior_path_side_shift_module/CMakeLists.txt b/planning/behavior_path_side_shift_module/CMakeLists.txt
new file mode 100644
index 0000000000000..28009e48a4cc4
--- /dev/null
+++ b/planning/behavior_path_side_shift_module/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_side_shift_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/scene.cpp
+  src/utils.cpp
+  src/manager.cpp
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
+    test/test_behavior_path_planner_node_interface.cpp
+  )
+
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/planning/behavior_path_planner/config/side_shift/side_shift.param.yaml b/planning/behavior_path_side_shift_module/config/side_shift.param.yaml
similarity index 100%
rename from planning/behavior_path_planner/config/side_shift/side_shift.param.yaml
rename to planning/behavior_path_side_shift_module/config/side_shift.param.yaml
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp
similarity index 86%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp
rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp
index 6475661f72ac4..c9edd3d6bf6a6 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp
+++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
+#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_
+#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_
 
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
@@ -51,4 +51,4 @@ struct SideShiftDebugData
 };
 
 }  // namespace behavior_path_planner
-#endif  // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_
+#endif  // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp
similarity index 83%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp
rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp
index 53bac3914ce86..ef8a48bc29760 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp
+++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp
@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_
+#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_
+#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_
 
-#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
+#include "behavior_path_side_shift_module/scene.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_
+#endif  // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp
similarity index 92%
rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp
index 014a0f403f9ec..7d72afebfb359 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
+++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021 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.
@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
+#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_
+#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_
 
-#include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp"
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_side_shift_module/data_structs.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -134,4 +134,4 @@ class SideShiftModule : public SceneModuleInterface
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
+#endif  // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp
similarity index 86%
rename from planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp
rename to planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp
index 6cdfd84d79075..210dd4e8a8d2a 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp
+++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021 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.
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
-#define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
+#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_
+#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -41,4 +41,4 @@ Point transformToGrid(
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__UTIL_HPP_
+#endif  // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_
diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml
new file mode 100644
index 0000000000000..5c421b04c0e39
--- /dev/null
+++ b/planning/behavior_path_side_shift_module/package.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>behavior_path_side_shift_module</name>
+  <version>0.1.0</version>
+  <description>The behavior_path_side_shift_module package</description>
+
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="tomohito.ando@tier4.jp">Tomohito Ando</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>behavior_path_planner</depend>
+  <depend>behavior_path_planner_common</depend>
+  <depend>geometry_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_planning_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/planning/behavior_path_side_shift_module/plugins.xml b/planning/behavior_path_side_shift_module/plugins.xml
new file mode 100644
index 0000000000000..2688e3a17c2b6
--- /dev/null
+++ b/planning/behavior_path_side_shift_module/plugins.xml
@@ -0,0 +1,3 @@
+<library path="behavior_path_side_shift_module">
+  <class type="behavior_path_planner::SideShiftModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
+</library>
diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_side_shift_module/src/manager.cpp
similarity index 97%
rename from planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp
rename to planning/behavior_path_side_shift_module/src/manager.cpp
index b20712ef19179..fa8d579da3d09 100644
--- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp
+++ b/planning/behavior_path_side_shift_module/src/manager.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/scene_module/side_shift/manager.hpp"
+#include "behavior_path_side_shift_module/manager.hpp"
 
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
rename to planning/behavior_path_side_shift_module/src/scene.cpp
index 9f77288e608cf..69df77672cd96 100644
--- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp
+++ b/planning/behavior_path_side_shift_module/src/scene.cpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021 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.
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+#include "behavior_path_side_shift_module/scene.hpp"
 
-#include "behavior_path_planner/utils/side_shift/util.hpp"
 #include "behavior_path_planner_common/marker_utils/utils.hpp"
 #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
+#include "behavior_path_side_shift_module/utils.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 
diff --git a/planning/behavior_path_planner/src/utils/side_shift/util.cpp b/planning/behavior_path_side_shift_module/src/utils.cpp
similarity index 97%
rename from planning/behavior_path_planner/src/utils/side_shift/util.cpp
rename to planning/behavior_path_side_shift_module/src/utils.cpp
index af733722562f8..a674d57597bc7 100644
--- a/planning/behavior_path_planner/src/utils/side_shift/util.cpp
+++ b/planning/behavior_path_side_shift_module/src/utils.cpp
@@ -1,4 +1,4 @@
-// Copyright 2021 Tier IV, Inc.
+// Copyright 2021 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.
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/utils/side_shift/util.hpp"
+#include "behavior_path_side_shift_module/utils.hpp"
 
 #include "behavior_path_planner_common/utils/utils.hpp"
 
diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp
new file mode 100644
index 0000000000000..94be083bdebcd
--- /dev/null
+++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp
@@ -0,0 +1,124 @@
+// Copyright 2023 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "behavior_path_planner/behavior_path_planner_node.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <vector>
+
+using behavior_path_planner::BehaviorPathPlannerNode;
+using planning_test_utils::PlanningInterfaceTestManager;
+
+std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
+{
+  auto test_manager = std::make_shared<PlanningInterfaceTestManager>();
+
+  // set subscriber with topic name: behavior_path_planner → test_node_
+  test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path");
+
+  // set behavior_path_planner's input topic name(this topic is changed to test node)
+  test_manager->setRouteInputTopicName("behavior_path_planner/input/route");
+
+  test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry");
+
+  return test_manager;
+}
+
+std::shared_ptr<BehaviorPathPlannerNode> generateNode()
+{
+  auto node_options = rclcpp::NodeOptions{};
+  const auto planning_test_utils_dir =
+    ament_index_cpp::get_package_share_directory("planning_test_utils");
+  const auto behavior_path_planner_dir =
+    ament_index_cpp::get_package_share_directory("behavior_path_planner");
+
+  std::vector<std::string> module_names;
+  module_names.emplace_back("behavior_path_planner::SideShiftModuleManager");
+
+  std::vector<rclcpp::Parameter> params;
+  params.emplace_back("launch_modules", module_names);
+  node_options.parameter_overrides(params);
+
+  test_utils::updateNodeOptions(
+    node_options, {planning_test_utils_dir + "/config/test_common.param.yaml",
+                   planning_test_utils_dir + "/config/test_nearest_search.param.yaml",
+                   planning_test_utils_dir + "/config/test_vehicle_info.param.yaml",
+                   behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml",
+                   behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
+                   behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
+                   ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") +
+                     "/config/side_shift.param.yaml"});
+
+  return std::make_shared<BehaviorPathPlannerNode>(node_options);
+}
+
+void publishMandatoryTopics(
+  std::shared_ptr<PlanningInterfaceTestManager> test_manager,
+  std::shared_ptr<BehaviorPathPlannerNode> test_target_node)
+{
+  // publish necessary topics from test_manager
+  test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry");
+  test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel");
+  test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception");
+  test_manager->publishOccupancyGrid(
+    test_target_node, "behavior_path_planner/input/occupancy_grid_map");
+  test_manager->publishLaneDrivingScenario(
+    test_target_node, "behavior_path_planner/input/scenario");
+  test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map");
+  test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap");
+  test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state");
+  test_manager->publishLateralOffset(
+    test_target_node, "behavior_path_planner/input/lateral_offset");
+}
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute)
+{
+  rclcpp::init(0, nullptr);
+  auto test_manager = generateTestManager();
+  auto test_target_node = generateNode();
+
+  publishMandatoryTopics(test_manager, test_target_node);
+
+  // test for normal trajectory
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
+  EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
+
+  // test with empty route
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node));
+  rclcpp::shutdown();
+}
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
+{
+  rclcpp::init(0, nullptr);
+
+  auto test_manager = generateTestManager();
+  auto test_target_node = generateNode();
+  publishMandatoryTopics(test_manager, test_target_node);
+
+  // test for normal trajectory
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node));
+
+  // make sure behavior_path_planner is running
+  EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
+
+  ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node));
+
+  rclcpp::shutdown();
+}