refactor(lane_change): separate lane change and external request (aut…

…owarefoundation#5850) * refactor(lane_change): separate lane change and external request Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactored external request lane change Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * separate on external request Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * style(pre-commit): autofix * delete external request Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix interface Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix external lane change couldn't be initialize Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix documents Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix link in README Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix based on comments Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * add ament auto package Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix test Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix AbLC test Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Interplai · May 28, 2024 · 0682af2 · 0682af2
1 parent 6cb720d
commit 0682af2
Show file tree

Hide file tree

Showing 55 changed files with 585 additions and 145 deletions.
diff --git a/planning/.pages b/planning/.pages
@@ -14,7 +14,7 @@ nav:
         - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design
         - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design
         - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design
-        - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design
+        - 'Lane Change': planning/behavior_path_lane_change_module
         - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design
         - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design
   - 'Behavior Velocity Planner':

diff --git a/...by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/...by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
 #include "behavior_path_avoidance_by_lane_change_module/scene.hpp"
-#include "behavior_path_planner/scene_module/lane_change/interface.hpp"
+#include "behavior_path_lane_change_module/interface.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 

diff --git a/...e_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/...e_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp
@@ -17,7 +17,7 @@
 
 #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
 #include "behavior_path_avoidance_by_lane_change_module/interface.hpp"
-#include "behavior_path_planner/scene_module/lane_change/manager.hpp"
+#include "behavior_path_lane_change_module/manager.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 

diff --git a/...nce_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/...nce_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
 
 #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp"
-#include "behavior_path_planner/scene_module/lane_change/normal.hpp"
+#include "behavior_path_lane_change_module/scene.hpp"
 
 #include <memory>
 

diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml
@@ -19,6 +19,7 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>behavior_path_avoidance_module</depend>
+  <depend>behavior_path_lane_change_module</depend>
   <depend>behavior_path_planner</depend>
   <depend>behavior_path_planner_common</depend>
   <depend>motion_utils</depend>

diff --git a/...r_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/...r_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp
@@ -47,6 +47,8 @@ std::shared_ptr<BehaviorPathPlannerNode> generateNode()
     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");
+  const auto behavior_path_lane_change_module_dir =
+    ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module");
 
   std::vector<std::string> module_names;
   module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager");
@@ -63,8 +65,7 @@ std::shared_ptr<BehaviorPathPlannerNode> generateNode()
      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_planner") +
-       "/config/lane_change/lane_change.param.yaml",
+     behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml",
      ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") +
        "/config/avoidance.param.yaml",
      ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") +

diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_external_request_lane_change_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/manager.cpp
+  src/scene.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()
diff --git a/..._lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/..._lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp
@@ -0,0 +1,54 @@
+// 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.
+
+#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_
+#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_
+
+#include "behavior_path_lane_change_module/manager.hpp"
+#include "route_handler/route_handler.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+
+namespace behavior_path_planner
+{
+class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager
+{
+public:
+  ExternalRequestLaneChangeRightModuleManager()
+  : LaneChangeModuleManager(
+      "external_request_lane_change_right", route_handler::Direction::RIGHT,
+      LaneChangeModuleType::EXTERNAL_REQUEST)
+  {
+  }
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+};
+
+class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager
+{
+public:
+  ExternalRequestLaneChangeLeftModuleManager()
+
+  : LaneChangeModuleManager(
+      "external_request_lane_change_left", route_handler::Direction::LEFT,
+      LaneChangeModuleType::EXTERNAL_REQUEST)
+  {
+  }
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_
diff --git a/...e_module/lane_change/external_request.hpp → ...rnal_request_lane_change_module/scene.hpp b/...e_module/lane_change/external_request.hpp → ...rnal_request_lane_change_module/scene.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
+#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_
+#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_
 
-#include "behavior_path_planner/scene_module/lane_change/normal.hpp"
+#include "behavior_path_lane_change_module/scene.hpp"
 
 #include <memory>
 
@@ -35,4 +35,4 @@ class ExternalRequestLaneChange : public NormalLaneChange
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__EXTERNAL_REQUEST_HPP_
+#endif  // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_
diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_external_request_lane_change_module/package.xml
@@ -0,0 +1,39 @@
+<?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_external_request_lane_change_module</name>
+  <version>0.1.0</version>
+  <description>The behavior_path_external_request_lane_change_module package</description>
+
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="zulfaqar.azmi@tier4.jp">Zulfaqar Azmi</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</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>behavior_path_lane_change_module</depend>
+  <depend>behavior_path_planner</depend>
+  <depend>behavior_path_planner_common</depend>
+  <depend>motion_utils</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rtc_interface</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>visualization_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_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml
@@ -0,0 +1,4 @@
+<library path="behavior_path_external_request_lane_change_module">
+  <class type="behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
+  <class type="behavior_path_planner::ExternalRequestLaneChangeRightModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
+</library>
diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_external_request_lane_change_module/src/manager.cpp
@@ -0,0 +1,49 @@
+// 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 "behavior_path_external_request_lane_change_module/manager.hpp"
+
+#include "behavior_path_external_request_lane_change_module/scene.hpp"
+#include "behavior_path_lane_change_module/interface.hpp"
+
+namespace behavior_path_planner
+{
+
+std::unique_ptr<SceneModuleInterface>
+ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance()
+{
+  return std::make_unique<LaneChangeInterface>(
+    name_, *node_, parameters_, rtc_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_,
+    std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
+}
+
+std::unique_ptr<SceneModuleInterface>
+ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance()
+{
+  return std::make_unique<LaneChangeInterface>(
+    name_, *node_, parameters_, rtc_interface_ptr_map_,
+    objects_of_interest_marker_interface_ptr_map_,
+    std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
+}
+
+}  // namespace behavior_path_planner
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  behavior_path_planner::ExternalRequestLaneChangeRightModuleManager,
+  behavior_path_planner::SceneModuleManagerInterface)
+PLUGINLIB_EXPORT_CLASS(
+  behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager,
+  behavior_path_planner::SceneModuleManagerInterface)
diff --git a/...e_module/lane_change/external_request.cpp → ..._request_lane_change_module/src/scene.cpp b/...e_module/lane_change/external_request.cpp → ..._request_lane_change_module/src/scene.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/lane_change/external_request.hpp"
+#include "behavior_path_external_request_lane_change_module/scene.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 

diff --git a/...th_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/...th_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp
@@ -0,0 +1,128 @@
+// 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");
+  const auto behavior_path_lane_change_module_dir =
+    ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module");
+
+  std::vector<std::string> module_names;
+  module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager");
+  module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager");
+
+  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",
+                    behavior_path_lane_change_module_dir + "/config/lane_change.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();
+}