feat(lane_change): add unit test for normal lane change class (RT1-7970)

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt

@@ -18,6 +18,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
     test/test_behavior_path_planner_node_interface.cpp
     test/test_lane_change_utils.cpp
+    test/test_lane_change_scene.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp

@@ -27,6 +27,7 @@
 
 namespace autoware::behavior_path_planner
 {
+using autoware::behavior_path_planner::lane_change::LCParamPtr;
 using autoware::route_handler::Direction;
 
 class LaneChangeModuleManager : public SceneModuleManagerInterface
@@ -44,6 +45,8 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
+  static LCParamPtr set_params(rclcpp::Node * node, const std::string & node_name);
+
 protected:
   void initParams(rclcpp::Node * node);
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

@@ -35,6 +35,7 @@
 #include <string>
 #include <utility>
 
+class TestNormalLaneChange;
 namespace autoware::behavior_path_planner
 {
 using autoware::route_handler::Direction;
@@ -296,6 +297,8 @@ class LaneChangeBase
   mutable rclcpp::Clock clock_{RCL_ROS_TIME};
 
   mutable std::shared_ptr<universe_utils::TimeKeeper> time_keeper_;
+
+  friend class ::TestNormalLaneChange;
 };
 }  // namespace autoware::behavior_path_planner
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -34,269 +34,274 @@
   initParams(node);
 }
 
-void LaneChangeModuleManager::initParams(rclcpp::Node * node)
+LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::string & node_name)
 {
   using autoware::universe_utils::getOrDeclareParameter;
 
   LaneChangeParameters p{};
 
   const auto parameter = [](std::string && name) { return "lane_change." + name; };
 
   // trajectory generation
   p.backward_lane_length = getOrDeclareParameter<double>(*node, parameter("backward_lane_length"));
   p.prediction_time_resolution =
     getOrDeclareParameter<double>(*node, parameter("prediction_time_resolution"));
   p.longitudinal_acc_sampling_num =
     getOrDeclareParameter<int>(*node, parameter("longitudinal_acceleration_sampling_num"));
   p.lateral_acc_sampling_num =
     getOrDeclareParameter<int>(*node, parameter("lateral_acceleration_sampling_num"));
 
   // parked vehicle detection
   p.object_check_min_road_shoulder_width =
     getOrDeclareParameter<double>(*node, parameter("object_check_min_road_shoulder_width"));
   p.object_shiftable_ratio_threshold =
     getOrDeclareParameter<double>(*node, parameter("object_shiftable_ratio_threshold"));
 
   // turn signal
   p.min_length_for_turn_signal_activation =
     getOrDeclareParameter<double>(*node, parameter("min_length_for_turn_signal_activation"));
   p.length_ratio_for_turn_signal_deactivation =
     getOrDeclareParameter<double>(*node, parameter("length_ratio_for_turn_signal_deactivation"));
 
   // acceleration
   p.min_longitudinal_acc = getOrDeclareParameter<double>(*node, parameter("min_longitudinal_acc"));
   p.max_longitudinal_acc = getOrDeclareParameter<double>(*node, parameter("max_longitudinal_acc"));
 
   // collision check
   p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter<bool>(
     *node, parameter("enable_collision_check_for_prepare_phase.general_lanes"));
   p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter<bool>(
     *node, parameter("enable_collision_check_for_prepare_phase.intersection"));
   p.enable_collision_check_for_prepare_phase_in_turns =
     getOrDeclareParameter<bool>(*node, parameter("enable_collision_check_for_prepare_phase.turns"));
   p.stopped_object_velocity_threshold =
     getOrDeclareParameter<double>(*node, parameter("stopped_object_velocity_threshold"));
   p.check_objects_on_current_lanes =
     getOrDeclareParameter<bool>(*node, parameter("check_objects_on_current_lanes"));
   p.check_objects_on_other_lanes =
     getOrDeclareParameter<bool>(*node, parameter("check_objects_on_other_lanes"));
   p.use_all_predicted_path =
     getOrDeclareParameter<bool>(*node, parameter("use_all_predicted_path"));
   p.lane_expansion_left_offset =
     getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.left_offset"));
   p.lane_expansion_right_offset =
     getOrDeclareParameter<double>(*node, parameter("safety_check.lane_expansion.right_offset"));
   // lane change regulations
   p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
   p.regulate_on_intersection =
     getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));
   p.regulate_on_traffic_light =
     getOrDeclareParameter<bool>(*node, parameter("regulation.traffic_light"));
 
   // ego vehicle stuck detection
   p.stop_velocity_threshold =
     getOrDeclareParameter<double>(*node, parameter("stuck_detection.velocity"));
   p.stop_time_threshold =
     getOrDeclareParameter<double>(*node, parameter("stuck_detection.stop_time"));
 
   // safety check
   p.allow_loose_check_for_cancel =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
   p.enable_target_lane_bound_check =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.enable_target_lane_bound_check"));
   p.collision_check_yaw_diff_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.collision_check_yaw_diff_threshold"));
 
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));
   p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_velocity_delta_time"));
   p.rss_params.front_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.expected_front_deceleration"));
   p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.expected_rear_deceleration"));
   p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.rear_vehicle_reaction_time"));
   p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin"));
   p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.lateral_distance_max_threshold"));
 
   p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
   p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.longitudinal_distance_min_threshold"));
   p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.longitudinal_velocity_delta_time"));
   p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.expected_front_deceleration"));
   p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.expected_rear_deceleration"));
   p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.rear_vehicle_reaction_time"));
   p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin"));
   p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.parked.lateral_distance_max_threshold"));
 
   p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold"));
   p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time"));
   p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.expected_front_deceleration"));
   p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.expected_rear_deceleration"));
   p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.rear_vehicle_reaction_time"));
   p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin"));
   p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.cancel.lateral_distance_max_threshold"));
 
   p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold"));
   p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time"));
   p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.expected_front_deceleration"));
   p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.expected_rear_deceleration"));
   p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.rear_vehicle_reaction_time"));
   p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin"));
   p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.stuck.lateral_distance_max_threshold"));
 
   // lane change parameters
   const auto max_acc = getOrDeclareParameter<double>(*node, "normal.max_acc");
   p.backward_length_buffer_for_end_of_lane =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_end_of_lane"));
   p.backward_length_buffer_for_blocking_object =
     getOrDeclareParameter<double>(*node, parameter("backward_length_buffer_for_blocking_object"));
   p.lane_changing_lateral_jerk =
     getOrDeclareParameter<double>(*node, parameter("lane_changing_lateral_jerk"));
   p.lane_change_prepare_duration =
     getOrDeclareParameter<double>(*node, parameter("prepare_duration"));
   p.minimum_lane_changing_velocity =
     getOrDeclareParameter<double>(*node, parameter("minimum_lane_changing_velocity"));
   p.minimum_lane_changing_velocity =
     std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration);
 
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
-      node->get_logger().get_child(name()),
+      node->get_logger().get_child(node_name),
       "Lane change buffer must be more than 1 meter. Modifying the buffer.");
   }
 
   // lateral acceleration map for lane change
   const auto lateral_acc_velocity =
     getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.velocity"));
   const auto min_lateral_acc =
     getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.min_values"));
   const auto max_lateral_acc =
     getOrDeclareParameter<std::vector<double>>(*node, parameter("lateral_acceleration.max_values"));
   if (
     lateral_acc_velocity.size() != min_lateral_acc.size() ||
     lateral_acc_velocity.size() != max_lateral_acc.size()) {
     RCLCPP_ERROR(
-      node->get_logger().get_child(name()),
+      node->get_logger().get_child(node_name),
       "Lane change lateral acceleration map has invalid size.");
     exit(EXIT_FAILURE);
   }
   for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) {
     p.lane_change_lat_acc_map.add(
       lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i));
   }
 
   // target object
   {
     const std::string ns = "lane_change.target_object.";
     p.object_types_to_check.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
     p.object_types_to_check.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
     p.object_types_to_check.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
     p.object_types_to_check.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
     p.object_types_to_check.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
     p.object_types_to_check.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
     p.object_types_to_check.check_motorcycle =
       getOrDeclareParameter<bool>(*node, ns + "motorcycle");
     p.object_types_to_check.check_pedestrian =
       getOrDeclareParameter<bool>(*node, ns + "pedestrian");
   }
 
   // lane change cancel
   p.cancel.enable_on_prepare_phase =
     getOrDeclareParameter<bool>(*node, parameter("cancel.enable_on_prepare_phase"));
   p.cancel.enable_on_lane_changing_phase =
     getOrDeclareParameter<bool>(*node, parameter("cancel.enable_on_lane_changing_phase"));
   p.cancel.delta_time = getOrDeclareParameter<double>(*node, parameter("cancel.delta_time"));
   p.cancel.duration = getOrDeclareParameter<double>(*node, parameter("cancel.duration"));
   p.cancel.max_lateral_jerk =
     getOrDeclareParameter<double>(*node, parameter("cancel.max_lateral_jerk"));
   p.cancel.overhang_tolerance =
     getOrDeclareParameter<double>(*node, parameter("cancel.overhang_tolerance"));
   p.cancel.unsafe_hysteresis_threshold =
     getOrDeclareParameter<int>(*node, parameter("cancel.unsafe_hysteresis_threshold"));
   p.cancel.deceleration_sampling_num =
     getOrDeclareParameter<int>(*node, parameter("cancel.deceleration_sampling_num"));
 
   // finish judge parameters
   p.lane_change_finish_judge_buffer =
     getOrDeclareParameter<double>(*node, parameter("lane_change_finish_judge_buffer"));
   p.finish_judge_lateral_threshold =
     getOrDeclareParameter<double>(*node, parameter("finish_judge_lateral_threshold"));
   const auto finish_judge_lateral_angle_deviation =
     getOrDeclareParameter<double>(*node, parameter("finish_judge_lateral_angle_deviation"));
   p.finish_judge_lateral_angle_deviation =
     autoware::universe_utils::deg2rad(finish_judge_lateral_angle_deviation);
 
   // debug marker
   p.publish_debug_marker = getOrDeclareParameter<bool>(*node, parameter("publish_debug_marker"));
 
   // validation of parameters
   if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) {
     RCLCPP_FATAL_STREAM(
-      node->get_logger().get_child(name()),
+      node->get_logger().get_child(node_name),
       "lane_change_sampling_num must be positive integer. Given longitudinal parameter: "
         << p.longitudinal_acc_sampling_num
         << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl
         << "Terminating the program...");
     exit(EXIT_FAILURE);
   }
 
   // validation of safety check parameters
   // if loosely check is not allowed, lane change module will keep on chattering and canceling, and
   // false positive situation might  occur
   if (!p.allow_loose_check_for_cancel) {
     if (
       p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration ||
       p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration ||
       p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time ||
       p.rss_params.rear_vehicle_safety_time_margin >
         p.rss_params_for_abort.rear_vehicle_safety_time_margin ||
       p.rss_params.lateral_distance_max_threshold >
         p.rss_params_for_abort.lateral_distance_max_threshold ||
       p.rss_params.longitudinal_distance_min_threshold >
         p.rss_params_for_abort.longitudinal_distance_min_threshold ||
       p.rss_params.longitudinal_velocity_delta_time >
         p.rss_params_for_abort.longitudinal_velocity_delta_time) {
       RCLCPP_FATAL_STREAM(
-        node->get_logger().get_child(name()),
+        node->get_logger().get_child(node_name),
         "abort parameter might be loose... Terminating the program...");
       exit(EXIT_FAILURE);
     }
   }
   if (p.cancel.delta_time < 1.0) {
     RCLCPP_WARN_STREAM(
-      node->get_logger().get_child(name()),
+      node->get_logger().get_child(node_name),
       "cancel.delta_time: " << p.cancel.delta_time
                             << ", is too short. This could cause a danger behavior.");
   }
 
-  parameters_ = std::make_shared<LaneChangeParameters>(p);
+  return std::make_shared<lane_change::Parameters>(p);
+}
+
+void LaneChangeModuleManager::initParams(rclcpp::Node * node)
+{
+  parameters_ = set_params(node, name());
 }
 
 std::unique_ptr<SceneModuleInterface> LaneChangeModuleManager::createNewSceneModuleInstance()

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp

@@ -0,0 +1,229 @@
+// Copyright 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.
+// 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 "autoware/behavior_path_lane_change_module/manager.hpp"
+#include "autoware/behavior_path_lane_change_module/scene.hpp"
+#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp"
+#include "autoware/behavior_path_planner_common/data_manager.hpp"
+#include "autoware_test_utils/autoware_test_utils.hpp"
+#include "autoware_test_utils/mock_data_parser.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+using autoware::behavior_path_planner::LaneChangeModuleManager;
+using autoware::behavior_path_planner::LaneChangeModuleType;
+using autoware::behavior_path_planner::NormalLaneChange;
+using autoware::behavior_path_planner::PlannerData;
+using autoware::behavior_path_planner::lane_change::CommonDataPtr;
+using autoware::behavior_path_planner::lane_change::LCParamPtr;
+using autoware::behavior_path_planner::lane_change::RouteHandlerPtr;
+using autoware::route_handler::Direction;
+using autoware::route_handler::RouteHandler;
+using autoware::test_utils::get_absolute_path_to_config;
+using autoware::test_utils::get_absolute_path_to_lanelet_map;
+using autoware::test_utils::get_absolute_path_to_route;
+using autoware_map_msgs::msg::LaneletMapBin;
+using geometry_msgs::msg::Pose;
+using tier4_planning_msgs::msg::PathWithLaneId;
+
+class TestNormalLaneChange : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    init_param();
+    init_module();
+  }
+
+  void init_param()
+  {
+    auto node_options = get_node_options();
+    auto node = rclcpp::Node(name_, node_options);
+    planner_data_->init_parameters(node);
+    lc_param_ptr_ = LaneChangeModuleManager::set_params(&node, node.get_name());
+    planner_data_->route_handler = init_route_handler();
+
+    ego_pose_ = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0);
+    planner_data_->self_odometry = set_odometry(ego_pose_);
+    planner_data_->dynamic_object =
+      std::make_shared<autoware_perception_msgs::msg::PredictedObjects>();
+  }
+
+  void init_module()
+  {
+    normal_lane_change_ =
+      std::make_shared<NormalLaneChange>(lc_param_ptr_, lc_type_, lc_direction_);
+    normal_lane_change_->setData(planner_data_);
+    set_previous_approved_path();
+  }
+
+  [[nodiscard]] const CommonDataPtr & get_common_data_ptr() const
+  {
+    return normal_lane_change_->common_data_ptr_;
+  }
+
+  [[nodiscard]] rclcpp::NodeOptions get_node_options() const
+  {
+    auto node_options = rclcpp::NodeOptions{};
+
+    const auto common_param =
+      get_absolute_path_to_config(test_utils_dir_, "test_common.param.yaml");
+    const auto nearest_search_param =
+      get_absolute_path_to_config(test_utils_dir_, "test_nearest_search.param.yaml");
+    const auto vehicle_info_param =
+      get_absolute_path_to_config(test_utils_dir_, "test_vehicle_info.param.yaml");
+
+    std::string bpp_dir{"autoware_behavior_path_planner"};
+    const auto bpp_param = get_absolute_path_to_config(bpp_dir, "behavior_path_planner.param.yaml");
+    const auto drivable_area_expansion_param =
+      get_absolute_path_to_config(bpp_dir, "drivable_area_expansion.param.yaml");
+    const auto scene_module_manager_param =
+      get_absolute_path_to_config(bpp_dir, "scene_module_manager.param.yaml");
+
+    std::string lc_dir{"autoware_behavior_path_lane_change_module"};
+    const auto lc_param = get_absolute_path_to_config(lc_dir, "lane_change.param.yaml");
+
+    autoware::test_utils::updateNodeOptions(
+      node_options, {common_param, nearest_search_param, vehicle_info_param, bpp_param,
+                     drivable_area_expansion_param, scene_module_manager_param, lc_param});
+    return node_options;
+  }
+
+  [[nodiscard]] RouteHandlerPtr init_route_handler() const
+  {
+    std::string autoware_route_handler_dir{"autoware_route_handler"};
+    std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"};
+    std::string lanelet_map_filename{"2km_test.osm"};
+    const auto lanelet2_path =
+      get_absolute_path_to_lanelet_map(test_utils_dir_, lanelet_map_filename);
+    const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, 5.0);
+    auto route_handler_ptr = std::make_shared<RouteHandler>(map_bin_msg);
+    const auto rh_test_route =
+      get_absolute_path_to_route(autoware_route_handler_dir, lane_change_right_test_route_filename);
+    route_handler_ptr->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route));
+
+    return route_handler_ptr;
+  }
+
+  static std::shared_ptr<nav_msgs::msg::Odometry> set_odometry(const Pose & pose)
+  {
+    nav_msgs::msg::Odometry odom;
+    odom.pose.pose = pose;
+    return std::make_shared<nav_msgs::msg::Odometry>(odom);
+  }
+
+  void set_previous_approved_path()
+  {
+    normal_lane_change_->prev_module_output_.path = create_previous_approved_path();
+  }
+
+  [[nodiscard]] PathWithLaneId create_previous_approved_path() const
+  {
+    const auto & common_data_ptr = get_common_data_ptr();
+    const auto & route_handler_ptr = common_data_ptr->route_handler_ptr;
+    lanelet::ConstLanelet closest_lane;
+    const auto current_pose = planner_data_->self_odometry->pose.pose;
+    route_handler_ptr->getClosestLaneletWithinRoute(current_pose, &closest_lane);
+    const auto backward_distance = common_data_ptr->bpp_param_ptr->backward_path_length;
+    const auto forward_distance = common_data_ptr->bpp_param_ptr->forward_path_length;
+    const auto current_lanes = route_handler_ptr->getLaneletSequence(
+      closest_lane, current_pose, backward_distance, forward_distance);
+
+    return route_handler_ptr->getCenterLinePath(
+      current_lanes, 0.0, std::numeric_limits<double>::max());
+  }
+
+  void TearDown() override
+  {
+    normal_lane_change_ = nullptr;
+    lc_param_ptr_ = nullptr;
+    planner_data_ = nullptr;
+    rclcpp::shutdown();
+  }
+
+  LCParamPtr lc_param_ptr_;
+  std::shared_ptr<NormalLaneChange> normal_lane_change_;
+  std::shared_ptr<PlannerData> planner_data_ = std::make_shared<PlannerData>();
+  LaneChangeModuleType lc_type_{LaneChangeModuleType::NORMAL};
+  Direction lc_direction_{Direction::RIGHT};
+  std::string name_{"test_lane_change_scene"};
+  std::string test_utils_dir_{"autoware_test_utils"};
+  Pose ego_pose_;
+};
+
+TEST_F(TestNormalLaneChange, testBaseClassInitialize)
+{
+  const auto type = normal_lane_change_->getModuleType();
+  const auto type_str = normal_lane_change_->getModuleTypeStr();
+
+  ASSERT_EQ(type, LaneChangeModuleType::NORMAL);
+  const auto is_type_str = type_str == "NORMAL";
+  ASSERT_TRUE(is_type_str);
+
+  ASSERT_EQ(normal_lane_change_->getDirection(), Direction::RIGHT);
+
+  ASSERT_TRUE(get_common_data_ptr());
+
+  ASSERT_TRUE(get_common_data_ptr()->is_data_available());
+  ASSERT_FALSE(get_common_data_ptr()->is_lanes_available());
+}
+
+TEST_F(TestNormalLaneChange, testUpdateLanes)
+{
+  constexpr auto is_approved = true;
+
+  normal_lane_change_->update_lanes(is_approved);
+
+  ASSERT_FALSE(get_common_data_ptr()->is_lanes_available());
+
+  normal_lane_change_->update_lanes(!is_approved);
+
+  ASSERT_TRUE(get_common_data_ptr()->is_lanes_available());
+}
+
+TEST_F(TestNormalLaneChange, testGetPathWhenInvalid)
+{
+  constexpr auto is_approved = true;
+  normal_lane_change_->update_lanes(!is_approved);
+  normal_lane_change_->update_filtered_objects();
+  normal_lane_change_->update_transient_data();
+  normal_lane_change_->updateLaneChangeStatus();
+  const auto & lc_status = normal_lane_change_->getLaneChangeStatus();
+
+  ASSERT_FALSE(lc_status.is_valid_path);
+}
+
+TEST_F(TestNormalLaneChange, testGetPathWhenValid)
+{
+  constexpr auto is_approved = true;
+  ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0);
+  planner_data_->self_odometry = set_odometry(ego_pose_);
+  set_previous_approved_path();
+  normal_lane_change_->update_lanes(!is_approved);
+  normal_lane_change_->update_filtered_objects();
+  normal_lane_change_->update_transient_data();
+  const auto lane_change_required = normal_lane_change_->isLaneChangeRequired();
+
+  ASSERT_TRUE(lane_change_required);
+
+  normal_lane_change_->updateLaneChangeStatus();
+  const auto & lc_status = normal_lane_change_->getLaneChangeStatus();
+
+  ASSERT_TRUE(lc_status.is_valid_path);
+}

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

@@ -247,9 +247,9 @@ class SceneModuleManagerInterface
 
   virtual void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) = 0;
 
-protected:
   void initInterface(rclcpp::Node * node, const std::vector<std::string> & rtc_types);
 
+protected:
   virtual std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() = 0;
 
   rclcpp::Node * node_ = nullptr;

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -1063,8 +1063,19 @@
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data)
 {
+  if (path.points.empty() || !planner_data) {
+    return {};
+  }
+
   const auto & route_handler = planner_data->route_handler;
-  const auto & current_pose = planner_data->self_odometry->pose.pose;
+  const auto & self_odometry = planner_data->self_odometry;
+
+  if (!route_handler || !self_odometry) {
+    return {};
+  }
+
+  const auto & current_pose = self_odometry->pose.pose;
+
   const auto & p = planner_data->parameters;
 
   std::set<lanelet::Id> lane_ids;