refactor(avoidance, avoidance_by_lane_change): separate package (auto…

…warefoundation#5790)

* refactor(avoidance): separate package

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(AbLC): separate package

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(bpp): remove separate module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp): fix test error

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: karishma <karishma@interpl.ai>

planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_path_avoidance_by_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/scene.cpp
+  src/manager.cpp
+  src/interface.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)

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp

@@ -0,0 +1,33 @@
+// Copyright 2022 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_
+
+#include "behavior_path_avoidance_module/data_structs.hpp"
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+struct AvoidanceByLCParameters : public AvoidanceParameters
+{
+  // execute only when the target object longitudinal distance is larger than this param.
+  double execute_object_longitudinal_margin{0.0};
+
+  // execute only when lane change end point is before the object.
+  bool execute_only_when_lane_change_finish_before_object{false};
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp

@@ -0,0 +1,48 @@
+// 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_
+
+#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 <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+namespace behavior_path_planner
+{
+class AvoidanceByLaneChangeInterface : public LaneChangeInterface
+{
+public:
+  AvoidanceByLaneChangeInterface(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<LaneChangeParameters> & parameters,
+    const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+    std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+      objects_of_interest_marker_interface_ptr_map);
+
+  bool isExecutionRequested() const override;
+
+protected:
+  void updateRTCStatus(const double start_distance, const double finish_distance) override;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_

planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp

@@ -0,0 +1,52 @@
+// 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_
+#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_
+
+#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 <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+using route_handler::Direction;
+
+class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
+{
+public:
+  AvoidanceByLaneChangeModuleManager()
+  : LaneChangeModuleManager(
+      "avoidance_by_lc", route_handler::Direction::NONE,
+      LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE)
+  {
+  }
+
+  void init(rclcpp::Node * node) override;
+
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+
+private:
+  std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters_;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp → planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp

@@ -12,18 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
-#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
+#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_
+#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 <memory>
 
 namespace behavior_path_planner
 {
-using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using geometry_msgs::msg::Pose;
-using geometry_msgs::msg::Twist;
 using AvoidanceDebugData = DebugData;
 
 class AvoidanceByLaneChange : public NormalLaneChange
@@ -56,4 +54,4 @@ class AvoidanceByLaneChange : public NormalLaneChange
 };
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_
+#endif  // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_

planning/behavior_path_avoidance_by_lane_change_module/package.xml

@@ -0,0 +1,36 @@
+<?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_avoidance_by_lane_change_module</name>
+  <version>0.1.0</version>
+  <description>The behavior_path_avoidance_by_lane_change_module package</description>
+
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
+  <maintainer email="zulfaqar.azmi@tier4.jp">Zulfaqar Azmi</maintainer>
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</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_avoidance_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>

planning/behavior_path_avoidance_by_lane_change_module/plugins.xml

@@ -0,0 +1,3 @@
+<library path="behavior_path_avoidance_by_lane_change_module">
+  <class type="behavior_path_planner::AvoidanceByLaneChangeModuleManager" base_class_type="behavior_path_planner::SceneModuleManagerInterface"/>
+</library>

planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -0,0 +1,62 @@
+// 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_avoidance_by_lane_change_module/interface.hpp"
+
+#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
+#include "behavior_path_planner_common/marker_utils/utils.hpp"
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace behavior_path_planner
+{
+AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
+  const std::string & name, rclcpp::Node & node,
+  const std::shared_ptr<LaneChangeParameters> & parameters,
+  const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
+  const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
+  std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
+    objects_of_interest_marker_interface_ptr_map)
+: LaneChangeInterface{
+    name,
+    node,
+    parameters,
+    rtc_interface_ptr_map,
+    objects_of_interest_marker_interface_ptr_map,
+    std::make_unique<AvoidanceByLaneChange>(parameters, avoidance_by_lane_change_parameters)}
+{
+}
+
+bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
+{
+  return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired();
+}
+
+void AvoidanceByLaneChangeInterface::updateRTCStatus(
+  const double start_distance, const double finish_distance)
+{
+  const auto direction = std::invoke([&]() -> std::string {
+    const auto dir = module_type_->getDirection();
+    return (dir == Direction::LEFT) ? "left" : "right";
+  });
+
+  rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
+    uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
+}
+}  // namespace behavior_path_planner