refactor(behavior_velocity_planner): use pluginlib (autowarefoundatio…

…n#3734) (autowarefoundation#509) * refactor(behavior_velocity_planner): use pluginlib (autowarefoundation#3734) * feat: move interface Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move utilization Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: add plugin loader Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: use plugin interface for all scene module Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: remove debug code Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: update package dependency Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move crosswalk Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: add template package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move intersection Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move traffic light Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move blind spot Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move detection area Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move no stopping area Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move occlusion spot Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move occlusion spot test Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move out of lane Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move run out Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: move speed bump and virtual traffic light Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * move: common test Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * apply Kimura-san's patch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add find_package pcl explicitly Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
kosuke55 · May 24, 2023 · ed07138 · ed07138
1 parent 8ef80a6
commit ed07138
Show file tree

Hide file tree

Showing 164 changed files with 1,927 additions and 798 deletions.
diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_velocity_blind_spot_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
+
+find_package(PCL REQUIRED)
+
+include_directories(
+  include
+  SYSTEM
+  ${PCL_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/debug.cpp
+  src/manager.cpp
+  src/scene.cpp
+)
+
+ament_auto_package()
diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml
@@ -0,0 +1,40 @@
+<?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_velocity_blind_spot_module</name>
+  <version>0.1.0</version>
+  <description>The behavior_velocity_blind_spot_module package</description>
+
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="mamoru.sobue@tier4.jp">Mamoru Sobue</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>behavior_velocity_planner_common</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libboost-dev</depend>
+  <depend>motion_utils</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>route_handler</depend>
+  <depend>tf2</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>visualization_msgs</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_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml
@@ -0,0 +1,3 @@
+<library path="behavior_velocity_blind_spot_module">
+  <class type="behavior_velocity_planner::BlindSpotModulePlugin" base_class_type="behavior_velocity_planner::PluginInterface"/>
+</library>
diff --git a/...ner/src/scene_module/blind_spot/debug.cpp → ..._velocity_blind_spot_module/src/debug.cpp b/...ner/src/scene_module/blind_spot/debug.cpp → ..._velocity_blind_spot_module/src/debug.cpp
@@ -12,10 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "scene.hpp"
+
+#include <behavior_velocity_planner_common/utilization/debug.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/motion_utils.hpp>
-#include <scene_module/blind_spot/scene.hpp>
-#include <utilization/debug.hpp>
-#include <utilization/util.hpp>
 
 #include <string>
 

diff --git a/...r/src/scene_module/blind_spot/manager.cpp → ...elocity_blind_spot_module/src/manager.cpp b/...r/src/scene_module/blind_spot/manager.cpp → ...elocity_blind_spot_module/src/manager.cpp
@@ -12,12 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <scene_module/blind_spot/manager.hpp>
-#include <utilization/boost_geometry_helper.hpp>
-#include <utilization/util.hpp>
+#include "manager.hpp"
+
+#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
+#include <limits>
 #include <memory>
 #include <set>
 #include <string>
@@ -82,3 +84,7 @@ BlindSpotModuleManager::getModuleExpiredFunction(
 }
 
 }  // namespace behavior_velocity_planner
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface)
diff --git a/...clude/scene_module/blind_spot/manager.hpp → ...elocity_blind_spot_module/src/manager.hpp b/...clude/scene_module/blind_spot/manager.hpp → ...elocity_blind_spot_module/src/manager.hpp
@@ -12,12 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_
-#define SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_
+#ifndef MANAGER_HPP_
+#define MANAGER_HPP_
 
+#include "scene.hpp"
+
+#include <behavior_velocity_planner_common/plugin_interface.hpp>
+#include <behavior_velocity_planner_common/plugin_wrapper.hpp>
+#include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <scene_module/blind_spot/scene.hpp>
-#include <scene_module/scene_module_interface.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
@@ -41,6 +44,11 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
+
+class BlindSpotModulePlugin : public PluginWrapper<BlindSpotModuleManager>
+{
+};
+
 }  // namespace behavior_velocity_planner
 
-#endif  // SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_
+#endif  // MANAGER_HPP_
diff --git a/...ner/src/scene_module/blind_spot/scene.cpp → ..._velocity_blind_spot_module/src/scene.cpp b/...ner/src/scene_module/blind_spot/scene.cpp → ..._velocity_blind_spot_module/src/scene.cpp
@@ -12,14 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "scene.hpp"
+
+#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <lanelet2_extension/regulatory_elements/road_marking.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <scene_module/blind_spot/scene.hpp>
 #include <tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp>
-#include <utilization/boost_geometry_helper.hpp>
-#include <utilization/path_utilization.hpp>
-#include <utilization/util.hpp>
 
 #include <boost/geometry/algorithms/area.hpp>
 #include <boost/geometry/algorithms/distance.hpp>
@@ -30,6 +31,7 @@
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 #include <string>
 #include <type_traits>

diff --git a/...include/scene_module/blind_spot/scene.hpp → ..._velocity_blind_spot_module/src/scene.hpp b/...include/scene_module/blind_spot/scene.hpp → ..._velocity_blind_spot_module/src/scene.hpp
@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCENE_MODULE__BLIND_SPOT__SCENE_HPP_
-#define SCENE_MODULE__BLIND_SPOT__SCENE_HPP_
+#ifndef SCENE_HPP_
+#define SCENE_HPP_
 
+#include <behavior_velocity_planner_common/scene_module_interface.hpp>
+#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <behavior_velocity_planner_common/utilization/state_machine.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <scene_module/scene_module_interface.hpp>
-#include <utilization/boost_geometry_helper.hpp>
-#include <utilization/state_machine.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -229,4 +229,4 @@ class BlindSpotModule : public SceneModuleInterface
 };
 }  // namespace behavior_velocity_planner
 
-#endif  // SCENE_MODULE__BLIND_SPOT__SCENE_HPP_
+#endif  // SCENE_HPP_
diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(behavior_velocity_crosswalk_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
+
+find_package(PCL REQUIRED)
+
+include_directories(
+  include
+  SYSTEM
+  ${PCL_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/debug.cpp
+  src/manager.cpp
+  src/scene_crosswalk.cpp
+  src/scene_walkway.cpp
+  src/util.cpp
+)
+
+ament_auto_package()
diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml
@@ -0,0 +1,45 @@
+<?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_velocity_crosswalk_module</name>
+  <version>0.1.0</version>
+  <description>The behavior_velocity_crosswalk_module package</description>
+
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="satoshi.ota@tier4.jp">Satoshi Ota</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
+  <depend>behavior_velocity_planner_common</depend>
+  <depend>eigen</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>libboost-dev</depend>
+  <depend>motion_utils</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>route_handler</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tier4_api_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</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_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml
@@ -0,0 +1,4 @@
+<library path="behavior_velocity_crosswalk_module">
+  <class type="behavior_velocity_planner::CrosswalkModulePlugin" base_class_type="behavior_velocity_planner::PluginInterface"/>
+  <class type="behavior_velocity_planner::WalkwayModulePlugin" base_class_type="behavior_velocity_planner::PluginInterface"/>
+</library>
diff --git a/...nner/src/scene_module/crosswalk/debug.cpp → ...r_velocity_crosswalk_module/src/debug.cpp b/...nner/src/scene_module/crosswalk/debug.cpp → ...r_velocity_crosswalk_module/src/debug.cpp
@@ -12,11 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "scene_crosswalk.hpp"
+#include "scene_walkway.hpp"
+
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <motion_utils/motion_utils.hpp>
-#include <scene_module/crosswalk/scene_crosswalk.hpp>
-#include <scene_module/crosswalk/scene_walkway.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-#include <utilization/util.hpp>
 
 #include <vector>
 

diff --git a/...er/src/scene_module/crosswalk/manager.cpp → ...velocity_crosswalk_module/src/manager.cpp b/...er/src/scene_module/crosswalk/manager.cpp → ...velocity_crosswalk_module/src/manager.cpp
@@ -12,9 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <scene_module/crosswalk/manager.hpp>
-#include <utilization/util.hpp>
+#include "manager.hpp"
 
+#include <behavior_velocity_planner_common/utilization/util.hpp>
+
+#include <limits>
 #include <memory>
 #include <set>
 #include <string>
@@ -195,3 +197,9 @@ WalkwayModuleManager::getModuleExpiredFunction(
   };
 }
 }  // namespace behavior_velocity_planner
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface)
+PLUGINLIB_EXPORT_CLASS(
+  behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface)
diff --git a/...nclude/scene_module/crosswalk/manager.hpp → ...velocity_crosswalk_module/src/manager.hpp b/...nclude/scene_module/crosswalk/manager.hpp → ...velocity_crosswalk_module/src/manager.hpp
@@ -12,13 +12,16 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCENE_MODULE__CROSSWALK__MANAGER_HPP_
-#define SCENE_MODULE__CROSSWALK__MANAGER_HPP_
+#ifndef MANAGER_HPP_
+#define MANAGER_HPP_
 
+#include "scene_crosswalk.hpp"
+#include "scene_walkway.hpp"
+
+#include <behavior_velocity_planner_common/plugin_interface.hpp>
+#include <behavior_velocity_planner_common/plugin_wrapper.hpp>
+#include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <scene_module/crosswalk/scene_crosswalk.hpp>
-#include <scene_module/crosswalk/scene_walkway.hpp>
-#include <scene_module/scene_module_interface.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_api_msgs/msg/crosswalk_status.hpp>
@@ -61,6 +64,15 @@ class WalkwayModuleManager : public SceneModuleManagerInterface
   std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 };
+
+class CrosswalkModulePlugin : public PluginWrapper<CrosswalkModuleManager>
+{
+};
+
+class WalkwayModulePlugin : public PluginWrapper<WalkwayModuleManager>
+{
+};
+
 }  // namespace behavior_velocity_planner
 
-#endif  // SCENE_MODULE__CROSSWALK__MANAGER_HPP_
+#endif  // MANAGER_HPP_
diff --git a/...cene_module/crosswalk/scene_crosswalk.cpp → ..._crosswalk_module/src/scene_crosswalk.cpp b/...cene_module/crosswalk/scene_crosswalk.cpp → ..._crosswalk_module/src/scene_crosswalk.cpp
@@ -12,14 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "scene_crosswalk.hpp"
+
 #include <autoware_auto_tf2/tf2_autoware_auto_msgs.hpp>
+#include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
+#include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <scene_module/crosswalk/scene_crosswalk.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
-#include <utilization/path_utilization.hpp>
-#include <utilization/util.hpp>
 
+#include <algorithm>
 #include <cmath>
+#include <limits>
+#include <set>
 #include <vector>
 
 namespace behavior_velocity_planner

diff --git a/...cene_module/crosswalk/scene_crosswalk.hpp → ..._crosswalk_module/src/scene_crosswalk.hpp b/...cene_module/crosswalk/scene_crosswalk.hpp → ..._crosswalk_module/src/scene_crosswalk.hpp
@@ -12,14 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_
-#define SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_
+#ifndef SCENE_CROSSWALK_HPP_
+#define SCENE_CROSSWALK_HPP_
 
+#include "util.hpp"
+
+#include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <scene_module/crosswalk/util.hpp>
-#include <scene_module/scene_module_interface.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -178,4 +179,4 @@ class CrosswalkModule : public SceneModuleInterface
 };
 }  // namespace behavior_velocity_planner
 
-#endif  // SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_
+#endif  // SCENE_CROSSWALK_HPP_