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

…70) (autowarefoundation#9090) * RT1-7970 testing base class Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * additional test Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Added update lanes Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * check path generation Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * check is lane change required Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix PRs comment Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
tier4 · Dec 20, 2024 · d69f12f · d69f12f
1 parent c6026a5
commit d69f12f
Show file tree

Hide file tree

Showing 7 changed files with 261 additions and 8 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/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}

diff --git a/...ior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp b/...ior_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);
 

diff --git a/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/...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_
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -34,7 +34,7 @@ void LaneChangeModuleManager::init(rclcpp::Node * node)
   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;
 
@@ -190,7 +190,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
 
   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.");
   }
 
@@ -205,7 +205,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
     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);
   }
@@ -261,7 +261,7 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   // 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
@@ -286,19 +286,24 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
       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()

diff --git a/...or_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/...or_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);
+}
diff --git a/...nclude/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/...nclude/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -327,6 +327,7 @@ class SceneModuleManagerInterface
     }
   }
 
+protected:
   virtual std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() = 0;
 
   rclcpp::Node * node_ = nullptr;

diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp
@@ -1255,8 +1255,19 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr<const PlannerData>
 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;
-Original file line number
+Diff line change
@@ Expand Up / @@ -327,6 +327,7 @@ class SceneModuleManagerInterface @@
         }
       }
+    protected:
       virtual std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() = 0;
       rclcpp::Node * node_ = nullptr;
@@ Expand Down @@