diff --git a/planning/behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_blind_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..72c9d7ce99a40 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..9485f165849f2 --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -0,0 +1,40 @@ + + + + behavior_velocity_blind_spot_module + 0.1.0 + The behavior_velocity_blind_spot_module package + + Mamoru Sobue + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml new file mode 100644 index 0000000000000..7dda59ea2fdbe --- /dev/null +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp rename to planning/behavior_velocity_blind_spot_module/src/debug.cpp index a8277c52474c6..32c91d2ac47ed 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp +++ b/planning/behavior_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 +#include #include -#include -#include -#include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp similarity index 89% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp rename to planning/behavior_velocity_blind_spot_module/src/manager.cpp index a91d8ea32212c..14fc5ea2fa552 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_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 -#include -#include +#include "manager.hpp" + +#include +#include #include +#include #include #include #include @@ -82,3 +84,7 @@ BlindSpotModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp rename to planning/behavior_velocity_blind_spot_module/src/manager.hpp index dccb2616edd71..9aeaa0abfc4b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp +++ b/planning/behavior_velocity_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 +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class BlindSpotModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp rename to planning/behavior_velocity_blind_spot_module/src/scene.cpp index 1e8795036bd0c..440c360e712a3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_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 +#include +#include #include #include #include -#include #include -#include -#include -#include #include #include @@ -30,6 +31,7 @@ #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp rename to planning/behavior_velocity_blind_spot_module/src/scene.hpp index 13810f503e42a..626879d3d3779 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_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 +#include +#include #include -#include -#include -#include #include #include @@ -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 new file mode 100644 index 0000000000000..d11c7263943af --- /dev/null +++ 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 new file mode 100644 index 0000000000000..e9b38f86a00ee --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -0,0 +1,45 @@ + + + + behavior_velocity_crosswalk_module + 0.1.0 + The behavior_velocity_crosswalk_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pcl_conversions + pluginlib + rclcpp + route_handler + sensor_msgs + tier4_api_msgs + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml new file mode 100644 index 0000000000000..6ab5b00758f29 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp rename to planning/behavior_velocity_crosswalk_module/src/debug.cpp index c6c7b378f2ac7..0f902cdcf0848 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp +++ b/planning/behavior_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 #include -#include -#include #include -#include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp rename to planning/behavior_velocity_crosswalk_module/src/manager.cpp index b60d0e87b6852..37d30e4e386bf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/planning/behavior_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 -#include +#include "manager.hpp" +#include + +#include #include #include #include @@ -195,3 +197,9 @@ WalkwayModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +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/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp similarity index 80% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp rename to planning/behavior_velocity_crosswalk_module/src/manager.hpp index 2c3f30cac048e..434c7eed9f065 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp +++ b/planning/behavior_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 +#include +#include #include -#include -#include -#include #include #include @@ -61,6 +64,15 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class CrosswalkModulePlugin : public PluginWrapper +{ +}; + +class WalkwayModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp rename to planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index a8d2391b4228d..6b2f15fe15719 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_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 +#include +#include #include -#include #include -#include -#include +#include #include +#include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp rename to planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 2de34959ab5cb..72c03f62b207c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp +++ b/planning/behavior_velocity_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 #include #include #include -#include -#include #include #include @@ -178,4 +179,4 @@ class CrosswalkModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ +#endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp rename to planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp index 7b968552ce654..ece8531e81f10 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_walkway.hpp" + +#include #include -#include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp similarity index 88% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp rename to planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp index ae06e478b9cfa..cb7dd9f47d303 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_walkway.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ -#define SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ +#ifndef SCENE_WALKWAY_HPP_ +#define SCENE_WALKWAY_HPP_ +#include "scene_crosswalk.hpp" +#include "util.hpp" + +#include #include #include -#include -#include -#include #include #include @@ -78,4 +79,4 @@ class WalkwayModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ +#endif // SCENE_WALKWAY_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp rename to planning/behavior_velocity_crosswalk_module/src/util.cpp index 385552ba75206..9a32f96838a92 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "util.hpp" + +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp b/planning/behavior_velocity_crosswalk_module/src/util.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp rename to planning/behavior_velocity_crosswalk_module/src/util.hpp index c99a815e577ee..56c97b4d9f91a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__CROSSWALK__UTIL_HPP_ -#define SCENE_MODULE__CROSSWALK__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ #include #include @@ -27,10 +27,9 @@ #include #define EIGEN_MPL2_ONLY -#include "behavior_velocity_planner/planner_data.hpp" - #include #include +#include #include @@ -86,4 +85,4 @@ lanelet::Optional getStopLineFromMap( const std::string & attribute_name); } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__CROSSWALK__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_detection_area_module/CMakeLists.txt new file mode 100644 index 0000000000000..bbc2ec69a8e57 --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_detection_area_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_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml new file mode 100644 index 0000000000000..66b8a62e83a2c --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_velocity_detection_area_module + 0.1.0 + The behavior_velocity_detection_area_module package + + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kyoichi Sugahara + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml new file mode 100644 index 0000000000000..73497c8bfdf2a --- /dev/null +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp rename to planning/behavior_velocity_detection_area_module/src/debug.cpp index 25da5766feff0..6316de449a44c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_detection_area_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 +#include #include -#include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp rename to planning/behavior_velocity_detection_area_module/src/manager.cpp index e09f46b627eeb..bde64c48f286d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "manager.hpp" + #include -#include #include +#include #include #include #include @@ -77,3 +79,7 @@ DetectionAreaModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp rename to planning/behavior_velocity_detection_area_module/src/manager.hpp index 0da425e208efe..10fca7182d09a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp +++ b/planning/behavior_velocity_detection_area_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__DETECTION_AREA__MANAGER_HPP_ -#define SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class DetectionAreaModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp rename to planning/behavior_velocity_detection_area_module/src/scene.cpp index c50ac84d24a3d..bac0d58a5e916 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utilization/arc_lane_util.hpp" +#include "scene.hpp" +#include +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp rename to planning/behavior_velocity_detection_area_module/src/scene.hpp index 687fa92e920fc..75aa09dd41317 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ -#define SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include @@ -23,10 +23,10 @@ #define EIGEN_MPL2_ONLY #include +#include +#include #include #include -#include -#include #include #include @@ -106,4 +106,4 @@ class DetectionAreaModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt new file mode 100644 index 0000000000000..9f0c5099d36e0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_intersection_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_intersection.cpp + src/scene_merge_from_private_road.cpp + src/util.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml new file mode 100644 index 0000000000000..25df9f2385d74 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -0,0 +1,53 @@ + + + + behavior_velocity_intersection_module + 0.1.0 + The behavior_velocity_intersection_module package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + cv_bridge + geometry_msgs + grid_map_cv + grid_map_ros + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + motion_utils + nav_msgs + pluginlib + rclcpp + route_handler + rtc_interface + tf2_geometry_msgs + tier4_api_msgs + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + grid_map_rviz_plugin + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml new file mode 100644 index 0000000000000..206f0324231ec --- /dev/null +++ b/planning/behavior_velocity_intersection_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp rename to planning/behavior_velocity_intersection_module/src/debug.cpp index ed62e8c6e7350..bdfb75cf45da0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include #include -#include -#include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp rename to planning/behavior_velocity_intersection_module/src/manager.cpp index bbdd96ade5b4e..02293c5634342 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -12,16 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "manager.hpp" + +#include +#include #include -#include -#include -#include #include +#include #include #include #include +#include #include namespace behavior_velocity_planner @@ -358,3 +361,10 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::MergeFromPrivateModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp similarity index 83% rename from planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp rename to planning/behavior_velocity_intersection_module/src/manager.hpp index c55f7e7f3f58d..ff9302db0b6af 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_intersection_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__INTERSECTION__MANAGER_HPP_ -#define SCENE_MODULE__INTERSECTION__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include +#include #include -#include -#include -#include #include #include @@ -74,6 +77,15 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; + +class IntersectionModulePlugin : public PluginWrapper +{ +}; + +class MergeFromPrivateModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp rename to planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 6d6233c3b16e6..9f6eee5829946 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -22,20 +22,23 @@ #include // #include // #include +#include "scene_intersection.hpp" +#include "util.hpp" + +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include #include #include #include +#include #include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp rename to planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a08a073bb5036..3f8253ab26157 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ -#define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#ifndef SCENE_INTERSECTION_HPP_ +#define SCENE_INTERSECTION_HPP_ +#include "util_type.hpp" + +#include +#include +#include #include #include #include -#include -#include #include -#include -#include #include #include @@ -332,4 +333,4 @@ class IntersectionModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ +#endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp rename to planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 58040fc18a626..9e2a3b9535c2a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -12,18 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_merge_from_private_road.hpp" + +#include "util.hpp" + +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp rename to planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index c631a96c127b8..cea85e3b2daf2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#define SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#include "scene_intersection.hpp" + +#include +#include +#include #include -#include -#include #include -#include -#include #include #include @@ -109,4 +110,4 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp rename to planning/behavior_velocity_intersection_module/src/util.cpp index 085503426ed88..98e3668ad0485 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "util.hpp" + +#include +#include #include #include #include #include #include -#include -#include -#include #include @@ -32,6 +33,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp rename to planning/behavior_velocity_intersection_module/src/util.hpp index 10c06f31376d9..77d390d09ebf1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__UTIL_HPP_ -#define SCENE_MODULE__INTERSECTION__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ + +#include "scene_intersection.hpp" +#include "util_type.hpp" #include -#include -#include #include @@ -193,4 +194,4 @@ std::vector generateDetectionLaneDivisions( } // namespace util } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__INTERSECTION__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp rename to planning/behavior_velocity_intersection_module/src/util_type.hpp index bf3a906cc40ca..5425c5dab4507 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ -#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#ifndef UTIL_TYPE_HPP_ +#define UTIL_TYPE_HPP_ #include #include @@ -56,4 +56,4 @@ struct DetectionLaneDivision } // namespace behavior_velocity_planner::util -#endif // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt new file mode 100644 index 0000000000000..d0778ad675da1 --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_no_stopping_area_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_no_stopping_area.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml new file mode 100644 index 0000000000000..a9c21f1aba189 --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -0,0 +1,46 @@ + + + + behavior_velocity_no_stopping_area_module + 0.1.0 + The behavior_velocity_no_stopping_area_module package + + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + interpolation + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml new file mode 100644 index 0000000000000..a9b07297cfa30 --- /dev/null +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index abfb44d795189..ad6ef3afc38f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "utilization/debug.hpp" - -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" -#include "utilization/util.hpp" +#include "scene_no_stopping_area.hpp" +#include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index e0a3b207d72ee..e290a70c00386 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/no_stopping_area/manager.hpp" +#include "manager.hpp" #include #include +#include #include #include #include @@ -81,3 +82,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp similarity index 72% rename from planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp rename to planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index 0dc4dd7559a9d..f7a9a5433e900 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ -#define SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "scene_no_stopping_area.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include +#include +#include +#include + +#include #include #include @@ -41,6 +44,11 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class NoStoppingAreaModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp rename to planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index a9c9e583bbfd3..77fbb18be2b5b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" - -#include "utilization/arc_lane_util.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" +#include "scene_no_stopping_area.hpp" +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp rename to planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index e387b1b39cf2d..304b6b65adfe2 100644 --- a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ -#define SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ +#ifndef SCENE_NO_STOPPING_AREA_HPP_ +#define SCENE_NO_STOPPING_AREA_HPP_ #define EIGEN_MPL2_ONLY -#include "scene_module/scene_module_interface.hpp" -#include "utilization/boost_geometry_helper.hpp" -#include "utilization/state_machine.hpp" - #include +#include +#include +#include #include #include @@ -180,4 +179,4 @@ class NoStoppingAreaModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ +#endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..91b1416afad21 --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_occlusion_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/grid_utils.cpp + src/manager.cpp + src/occlusion_spot_utils.cpp + src/risk_predictive_braking.cpp + src/scene_occlusion_spot.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_occlusion_spot_utils.cpp + test/src/test_risk_predictive_braking.cpp + test/src/test_grid_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package() diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml new file mode 100644 index 0000000000000..cb5bd744edfa8 --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -0,0 +1,47 @@ + + + + behavior_velocity_occlusion_spot_module + 0.1.0 + The behavior_velocity_occlusion_spot_module package + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Taiki Tanaka + + ament_cmake_auto + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + motion_utils + nav_msgs + pluginlib + rclcpp + route_handler + tf2 + tf2_geometry_msgs + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml new file mode 100644 index 0000000000000..a38900d1a893b --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index bf82bd74552df..737d3bdee1d3e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include #include -#include -#include #include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index a9be8abce007f..69ad30e32dfdc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "grid_utils.hpp" #include +#include #include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 538d04cdadde6..849e16eb0834c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ +#ifndef GRID_UTILS_HPP_ +#define GRID_UTILS_HPP_ +#include +#include #include #include #include @@ -22,8 +24,6 @@ #include #include #include -#include -#include #include #include @@ -120,4 +120,4 @@ void denoiseOccupancyGridCV( } // namespace grid_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ +#endif // GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index b6a21dd0acee5..5b1b3d489041f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "manager.hpp" + +#include "scene_occlusion_spot.hpp" + +#include #include @@ -129,3 +131,7 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp similarity index 80% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index 7efb3b522560d..c153a727b8a7e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_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__OCCLUSION_SPOT__MANAGER_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include +#include #include -#include -#include -#include #include #include @@ -55,6 +58,11 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class OcclusionSpotModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index d8507cb5a77c8..0a172c7f5544b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" + +#include "risk_predictive_braking.hpp" + +#include +#include #include #include -#include -#include #include #include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 47ea7b5532278..cc07461864c16 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ +#ifndef OCCLUSION_SPOT_UTILS_HPP_ +#define OCCLUSION_SPOT_UTILS_HPP_ +#include "grid_utils.hpp" + +#include #include #include #include #include -#include -#include #include #include @@ -252,4 +253,4 @@ bool generatePossibleCollisionsFromGridMap( } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ +#endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index f4817428af445..ef181ee7613a0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "risk_predictive_braking.hpp" + +#include "occlusion_spot_utils.hpp" + +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 82% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 15eac76e35c48..9a0278bfad5e5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ +#ifndef RISK_PREDICTIVE_BRAKING_HPP_ +#define RISK_PREDICTIVE_BRAKING_HPP_ -#include +#include "occlusion_spot_utils.hpp" #include @@ -40,4 +40,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ +#endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp rename to planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index a37154c20f1d7..b7278aff29902 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -12,20 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene_occlusion_spot.hpp" + +#include "occlusion_spot_utils.hpp" +#include "risk_predictive_braking.hpp" + +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 86% rename from planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp rename to planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index c7ee714b03a7b..5632a713eec3c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#ifndef SCENE_OCCLUSION_SPOT_HPP_ +#define SCENE_OCCLUSION_SPOT_HPP_ +#include "occlusion_spot_utils.hpp" + +#include +#include #include -#include -#include #include -#include #include #include @@ -74,4 +75,4 @@ class OcclusionSpotModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_ +#endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_grid_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index 19e9b5f37b028..bc526959386e9 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 96% rename from planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 93f435590f3e0..ef2e776112766 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -13,12 +13,13 @@ // limitations under the License. #include "gtest/gtest.h" -#include "scene_module/occlusion_spot/occlusion_spot_utils.hpp" +#include "occlusion_spot_utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" #include "utils.hpp" +#include +#include + #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" diff --git a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 93% rename from planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp rename to planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 6f3a3e4100123..9b1d92b71a111 100644 --- a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "occlusion_spot_utils.hpp" +#include "risk_predictive_braking.hpp" #include "utils.hpp" -#include -#include - #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp new file mode 100644 index 0000000000000..cf24be4cc0b5c --- /dev/null +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include "occlusion_spot_utils.hpp" + +#include + +#include +#include + +#include + +namespace test +{ + +inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) +inline grid_map::GridMap generateGrid(int w, int h, double res) +{ + grid_map::GridMap grid{}; + grid_map::Length length(w * res, h * res); + grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); + grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + return grid; +} + +using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +inline void generatePossibleCollisions( + std::vector & possible_collisions, double x0, double y0, double x, + double y, int nb_cols) +{ + using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + const double lon = 0.0; // assume col_x = intersection_x + const double lat = -1.0; + const double velocity = 1.0; + /** + * @brief representation of a possible collision between ego and some obstacle + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ + double x_step = (x - x0) / (nb_cols - 1); + double y_step = (y - y0) / (nb_cols - 1); + for (int i = 0; i < nb_cols; ++i) { + // collision + ObstacleInfo obstacle_info; + obstacle_info.position.x = x0 + x_step * i; + obstacle_info.position.y = y0 + y_step * i + lat; + obstacle_info.max_velocity = velocity; + + // intersection + geometry_msgs::msg::Pose intersection_pose{}; + intersection_pose.position.x = x0 + x_step * i + lon; + intersection_pose.position.x = y0 + y_step * i; + + // collision path point + autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; + collision_with_margin.pose.position.x = x0 + x_step * i + lon; + collision_with_margin.pose.position.y = y0 + y_step * i; + + lanelet::ArcCoordinates arc; + arc.length = obstacle_info.position.x; + arc.distance = obstacle_info.position.y; + + PossibleCollisionInfo col(obstacle_info, collision_with_margin, intersection_pose, arc); + possible_collisions.emplace_back(col); + } +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt new file mode 100644 index 0000000000000..28d7b5c9f3c26 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_out_of_lane_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/decisions.cpp + src/footprint.cpp + src/lanelets_selection.cpp + src/manager.cpp + src/overlapping_range.cpp + src/scene_out_of_lane.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml new file mode 100644 index 0000000000000..c2b5c0310c9a7 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_out_of_lane_module + 0.1.0 + The behavior_velocity_out_of_lane_module package + + Maxime Clement + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml new file mode 100644 index 0000000000000..8c18fdce79480 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp rename to planning/behavior_velocity_out_of_lane_module/src/debug.cpp index eb69a0655a733..de0b78424f586 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/debug.hpp" +#include "debug.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp rename to planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 7385967bbb09a..06bc6d935f310 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -52,4 +52,4 @@ void add_lanelet_markers( const std_msgs::msg::ColorRGBA & color); } // namespace behavior_velocity_planner::out_of_lane::debug -#endif // SCENE_MODULE__OUT_OF_LANE__DEBUG_HPP_ +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp rename to planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 68e96ec3d25e4..a164a03a9aa43 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/decisions.hpp" +#include "decisions.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp rename to planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 345835ce53250..6af03bc53ea3c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ +#ifndef DECISIONS_HPP_ +#define DECISIONS_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include #include @@ -123,4 +123,4 @@ std::vector calculate_decisions( const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__DECISIONS_HPP_ +#endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp rename to planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 784b8d81e7847..929fbd944af9f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/footprint.hpp" +#include "footprint.hpp" #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp rename to planning/behavior_velocity_out_of_lane_module/src/footprint.hpp index feaa6663e3a23..a573b6ff3981c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/footprint.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ +#ifndef FOOTPRINT_HPP_ +#define FOOTPRINT_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -56,4 +56,4 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( } // namespace out_of_lane } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OUT_OF_LANE__FOOTPRINT_HPP_ +#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp similarity index 96% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp rename to planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 35ae9cf3bf00a..a0e2b86caa623 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/lanelets_selection.hpp" +#include "lanelets_selection.hpp" -#include "utilization/util.hpp" +#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp rename to planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index 90a92f6580abc..cd0b23768872d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ +#ifndef LANELETS_SELECTION_HPP_ +#define LANELETS_SELECTION_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -55,4 +55,4 @@ lanelet::ConstLanelets calculate_other_lanelets( const route_handler::RouteHandler & route_handler, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__LANELETS_SELECTION_HPP_ +#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp rename to planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 4c055e9bccfab..1eccd8773cc92 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include "manager.hpp" + +#include "scene_out_of_lane.hpp" + +#include #include @@ -85,3 +87,7 @@ OutOfLaneModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp similarity index 81% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp rename to planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 39e18655383c2..c8ef397913c8f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_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__OUT_OF_LANE__MANAGER_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene_out_of_lane.hpp" + +#include +#include +#include #include -#include -#include #include #include @@ -53,6 +56,11 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class OutOfLaneModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__OUT_OF_LANE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp rename to planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index fd872f10b8f97..98a9e4404e013 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/overlapping_range.hpp" +#include "overlapping_range.hpp" #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp rename to planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index 9b417b8efd3ee..ab4d31480e1cb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ +#ifndef OVERLAPPING_RANGE_HPP_ +#define OVERLAPPING_RANGE_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" #include @@ -66,4 +66,4 @@ OverlapRanges calculate_overlapping_ranges( const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__OVERLAPPING_RANGE_HPP_ +#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp rename to planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 6921f90ab3ffb..20260f4dc41f6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/out_of_lane/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -12,25 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/out_of_lane/scene_out_of_lane.hpp" +#include "scene_out_of_lane.hpp" -#include "scene_module/out_of_lane/debug.hpp" -#include "scene_module/out_of_lane/decisions.hpp" -#include "scene_module/out_of_lane/footprint.hpp" -#include "scene_module/out_of_lane/lanelets_selection.hpp" -#include "scene_module/out_of_lane/overlapping_range.hpp" -#include "scene_module/out_of_lane/types.hpp" +#include "debug.hpp" +#include "decisions.hpp" +#include "footprint.hpp" +#include "lanelets_selection.hpp" +#include "overlapping_range.hpp" +#include "types.hpp" +#include +#include #include #include #include -#include -#include #include #include #include +#include #include namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp rename to planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 01ab0856f5525..2d6a3b941cc33 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ +#ifndef SCENE_OUT_OF_LANE_HPP_ +#define SCENE_OUT_OF_LANE_HPP_ -#include "scene_module/out_of_lane/types.hpp" +#include "types.hpp" +#include #include -#include #include #include @@ -70,4 +70,4 @@ std::vector calculate_slowdown_points( } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__SCENE_OUT_OF_LANE_HPP_ +#endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp rename to planning/behavior_velocity_out_of_lane_module/src/types.hpp index 3bee7b1c5eded..e4ba8fbfead81 100644 --- a/planning/behavior_velocity_planner/include/scene_module/out_of_lane/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ -#define SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ +#ifndef TYPES_HPP_ +#define TYPES_HPP_ #include @@ -173,4 +173,4 @@ struct DebugData } // namespace behavior_velocity_planner::out_of_lane -#endif // SCENE_MODULE__OUT_OF_LANE__TYPES_HPP_ +#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 1c97d8c27c934..e51d5f0c36c42 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -4,97 +4,34 @@ project(behavior_velocity_planner) find_package(autoware_cmake REQUIRED) autoware_package() -# Find the non-obvious packages manually -find_package(Boost REQUIRED) -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common filters) -find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) -set(scene_modules - detection_area - blind_spot - stop_line - crosswalk - traffic_light - intersection - no_stopping_area - virtual_traffic_light - occlusion_spot - run_out - speed_bump - out_of_lane +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} ) -foreach(scene_module IN LISTS scene_modules) - file(GLOB_RECURSE scene_module_src "src/scene_module/${scene_module}/*") - list(APPEND scene_modules_src ${scene_module_src}) -endforeach() - -ament_auto_add_library(behavior_velocity_planner SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp src/planner_manager.cpp - src/utilization/path_utilization.cpp - src/utilization/util.cpp - src/utilization/debug.cpp - ${scene_modules_src} -) - -target_include_directories(behavior_velocity_planner - SYSTEM PUBLIC - ${BOOST_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${tf2_geometry_msgs_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} ) -ament_target_dependencies(behavior_velocity_planner - Boost - Eigen3 - PCL - message_filters -) - -target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES}) - -rclcpp_components_register_node(behavior_velocity_planner +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" - EXECUTABLE behavior_velocity_planner_node + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) - # Gtest for utilization - ament_add_ros_isolated_gtest(utilization-test - test/src/test_state_machine.cpp - test/src/test_arc_lane_util.cpp - test/src/test_utilization.cpp - ) - target_link_libraries(utilization-test - gtest_main - behavior_velocity_planner - ) - - # Gtest for occlusion spot - ament_add_ros_isolated_gtest(occlusion_spot-test - test/src/test_occlusion_spot_utils.cpp - test/src/test_risk_predictive_braking.cpp - test/src/test_grid_utils.cpp - ) - # Gtest for out of lane - # ament_add_ros_isolated_gtest(out_of_lane-test - # ) - target_link_libraries(occlusion_spot-test - gtest_main - behavior_velocity_planner - ) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_${PROJECT_NAME}_node_interface.cpp + test/src/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main ${PROJECT_NAME} + ${PCL_LIBRARIES} ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index ef4beb90a44d6..0f131eebcd40c 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -5,32 +5,16 @@ 0.1.0 The behavior_velocity_planner package - Mamoru Sobue Takayuki Murooka - Satoshi Ota - Kyoichi Sugahara - Taiki Tanaka - Kosuke Takeuchi - - Satoshi Ota - - Mamoru Sobue - Yutaka Shimizu - - Kosuke Takeuchi - Tomohito Ando Makoto Kurihara - Maxime Clement - - Tomoya Kimura Shumpei Wakabayashi @@ -45,37 +29,26 @@ Tomohito Ando Yukihiro Saito - ament_cmake + ament_cmake_auto autoware_cmake eigen3_cmake_module - autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 - cv_bridge + behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs - grid_map_cv - grid_map_ros - grid_map_utils - interpolation lanelet2_extension libboost-dev - libopencv-dev - magic_enum - message_filters motion_utils motion_velocity_smoother - nav_msgs - nlohmann-json-dev pcl_conversions + pluginlib rclcpp rclcpp_components route_handler - rtc_interface sensor_msgs tf2 tf2_eigen @@ -85,9 +58,7 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs - grid_map_rviz_plugin ament_cmake_ros ament_lint_auto diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 73f8c7d569e4d..5260385fa3b39 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_velocity_planner/node.hpp" +#include "node.hpp" +#include #include #include -#include #include #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -33,20 +34,6 @@ #include #include -// Scene modules -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -150,46 +137,53 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Initialize PlannerManager if (this->declare_parameter("launch_crosswalk")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::CrosswalkModulePlugin"); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::WalkwayModulePlugin"); } if (this->declare_parameter("launch_traffic_light")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::TrafficLightModulePlugin"); } if (this->declare_parameter("launch_intersection")) { // intersection module should be before merge from private to declare intersection parameters - planner_manager_.launchSceneModule(std::make_shared(*this)); - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::IntersectionModulePlugin"); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::MergeFromPrivateModulePlugin"); } if (this->declare_parameter("launch_blind_spot")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::BlindSpotModulePlugin"); } if (this->declare_parameter("launch_detection_area")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::DetectionAreaModulePlugin"); } if (this->declare_parameter("launch_virtual_traffic_light")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::VirtualTrafficLightModulePlugin"); } // this module requires all the stop line.Therefore this modules should be placed at the bottom. if (this->declare_parameter("launch_no_stopping_area")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::NoStoppingAreaModulePlugin"); } // permanent stop line module should be after no stopping area if (this->declare_parameter("launch_stop_line")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::StopLineModulePlugin"); } // to calculate ttc it's better to be after stop line if (this->declare_parameter("launch_occlusion_spot")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin( + *this, "behavior_velocity_planner::OcclusionSpotModulePlugin"); } if (this->declare_parameter("launch_run_out")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::RunOutModulePlugin"); } if (this->declare_parameter("launch_speed_bump")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::SpeedBumpModulePlugin"); } if (this->declare_parameter("launch_out_of_lane")) { - planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchScenePlugin(*this, "behavior_velocity_planner::OutOfLaneModulePlugin"); } } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/src/node.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp rename to planning/behavior_velocity_planner/src/node.hpp index 6e8f3a33c8d67..91172fdd77da8 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#ifndef NODE_HPP_ +#define NODE_HPP_ -#include "behavior_velocity_planner/planner_data.hpp" -#include "behavior_velocity_planner/planner_manager.hpp" +#include "planner_manager.hpp" +#include #include #include @@ -114,4 +114,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#endif // NODE_HPP_ diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 374ddaa234a77..39d4dc345e899 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_velocity_planner/planner_manager.hpp" +#include "planner_manager.hpp" #include @@ -49,10 +49,21 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } } // namespace -void BehaviorVelocityPlannerManager::launchSceneModule( - const std::shared_ptr & scene_module_manager_ptr) +BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() +: plugin_loader_("behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") { - scene_manager_ptrs_.push_back(scene_module_manager_ptr); +} + +void BehaviorVelocityPlannerManager::launchScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(node); + scene_manager_plugins_.push_back(plugin); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } } autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( @@ -64,15 +75,15 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: int first_stop_path_point_index = static_cast(output_path_msg.points.size() - 1); std::string stop_reason_msg("path_end"); - for (const auto & scene_manager_ptr : scene_manager_ptrs_) { - scene_manager_ptr->updateSceneModuleInstances(planner_data, input_path_msg); - scene_manager_ptr->plan(&output_path_msg); - boost::optional firstStopPathPointIndex = scene_manager_ptr->getFirstStopPathPointIndex(); + for (const auto & plugin : scene_manager_plugins_) { + plugin->updateSceneModuleInstances(planner_data, input_path_msg); + plugin->plan(&output_path_msg); + boost::optional firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); if (firstStopPathPointIndex) { if (firstStopPathPointIndex.get() < first_stop_path_point_index) { first_stop_path_point_index = firstStopPathPointIndex.get(); - stop_reason_msg = scene_manager_ptr->getModuleName(); + stop_reason_msg = plugin->getModuleName(); } } } diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp similarity index 77% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp rename to planning/behavior_velocity_planner/src/planner_manager.hpp index 3b24c72474e87..e57d90d5cdfee 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#ifndef PLANNER_MANAGER_HPP_ +#define PLANNER_MANAGER_HPP_ +#include +#include +#include #include -#include #include #include @@ -39,8 +41,8 @@ namespace behavior_velocity_planner class BehaviorVelocityPlannerManager { public: - void launchSceneModule( - const std::shared_ptr & scene_module_manager_ptr); + BehaviorVelocityPlannerManager(); + void launchScenePlugin(rclcpp::Node & node, const std::string & name); autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, @@ -49,9 +51,10 @@ class BehaviorVelocityPlannerManager diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; private: - std::vector> scene_manager_ptrs_; diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; + pluginlib::ClassLoader plugin_loader_; + std::vector> scene_manager_plugins_; }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#endif // PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp similarity index 99% rename from planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp rename to planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 00e670f0b0ea0..061a46f01f219 100644 --- a/planning/behavior_velocity_planner/test/src/test_behavior_velocity_planner_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" -#include "behavior_velocity_planner/node.hpp" +#include "node.hpp" #include "planning_interface_test_manager/planning_interface_test_manager.hpp" #include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" diff --git a/planning/behavior_velocity_planner/test/src/utils.hpp b/planning/behavior_velocity_planner/test/src/utils.hpp deleted file mode 100644 index 91574b02c65e2..0000000000000 --- a/planning/behavior_velocity_planner/test/src/utils.hpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright 2021 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 UTILS_HPP_ -#define UTILS_HPP_ - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace test -{ -// Default grid parameter such that UNKNOWN cells have a value of 50 -const behavior_velocity_planner::grid_utils::GridParam grid_param = {10, 51}; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using ConstPair = const std::pair; - -/* lanelet - 0 1 2 3 4 5 - 0 x - 1 x - 2 x - 3 x - 4 x - 5 x - */ -inline lanelet::ConstLanelet createLanelet( - ConstPair & start = {0, 0}, ConstPair & end = {5, 5}, int nb_points = 6) -{ - const double interval = - std::hypot(end.first - start.first, end.second - start.second) / (nb_points - 1); - const double norm_x = (end.first - start.first) / interval; - const double norm_y = (end.second - start.second) / interval; - lanelet::Points3d path_points; - for (int i = 0; i < nb_points; i++) { - const double x = start.first + norm_x * i; - const double y = start.second + norm_y * i; - path_points.emplace_back(lanelet::InvalId, x, y, 0); - } - lanelet::LineString3d centerline(lanelet::InvalId, path_points); - lanelet::Lanelet path_lanelet(lanelet::InvalId); - path_lanelet.setCenterline(centerline); - return lanelet::ConstLanelet(path_lanelet); -} - -/* Horizontal lanelet - 0 1 2 3 4 5 6 - 0 x x x x x x x - 1 - 2 x x x x x x x - 3 - 4 - 5 - */ -inline lanelet::ConstLanelet horizontalLanelet( - std::pair origin = {0, 0}, double width = 2.0, double length = 6.0, - int nb_points = 2) -{ - lanelet::Lanelet l; - lanelet::Points3d line; - for (double x = origin.first; x <= origin.first + length; x += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second)); - } - l.setLeftBound(lanelet::LineString3d(0, line)); - line.clear(); - for (double x = origin.second; x <= origin.first + length; x += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second + width)); - } - l.setRightBound(lanelet::LineString3d(1, line)); - return lanelet::ConstLanelet(l); -} -/* Vertical lanelet - 0 1 2 3 4 5 6 - 0 x x - 1 x x - 2 x x - 3 x x - 4 x x - 5 x x - */ -inline lanelet::ConstLanelet verticalLanelet( - std::pair origin = {0, 0}, double width = 2.0, double length = 5.0, - int nb_points = 2) -{ - lanelet::Lanelet l; - lanelet::Points3d line; - for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, origin.first, y)); - } - l.setLeftBound(lanelet::LineString3d(0, line)); - line.clear(); - for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { - line.emplace_back(lanelet::ConstPoint2d(0, origin.first + width, y)); - } - l.setRightBound(lanelet::LineString3d(1, line)); - return lanelet::ConstLanelet(l); -} - -// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) -inline grid_map::GridMap generateGrid(int w, int h, double res) -{ - grid_map::GridMap grid{}; - grid_map::Length length(w * res, h * res); - grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); - grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); - return grid; -} - -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( - double x0, double y0, double x, double y, int nb_points) -{ - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; - double x_step = (x - x0) / (nb_points - 1); - double y_step = (y - y0) / (nb_points - 1); - for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; - point.point.pose.position.x = x0 + x_step * i; - point.point.pose.position.y = y0 + y_step * i; - point.point.pose.position.z = 0.0; - path.points.push_back(point); - } - return path; -} - -using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; -inline void generatePossibleCollisions( - std::vector & possible_collisions, double x0, double y0, double x, - double y, int nb_cols) -{ - using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; - const double lon = 0.0; // assume col_x = intersection_x - const double lat = -1.0; - const double velocity = 1.0; - /** - * @brief representation of a possible collision between ego and some obstacle - * ^ - * | - * Ego ---------collision----------intersection-------> path - * | - * ------------------ | - * | Vehicle | obstacle - * ------------------ - */ - double x_step = (x - x0) / (nb_cols - 1); - double y_step = (y - y0) / (nb_cols - 1); - for (int i = 0; i < nb_cols; ++i) { - // collision - ObstacleInfo obstacle_info; - obstacle_info.position.x = x0 + x_step * i; - obstacle_info.position.y = y0 + y_step * i + lat; - obstacle_info.max_velocity = velocity; - - // intersection - geometry_msgs::msg::Pose intersection_pose{}; - intersection_pose.position.x = x0 + x_step * i + lon; - intersection_pose.position.x = y0 + y_step * i; - - // collision path point - autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; - collision_with_margin.pose.position.x = x0 + x_step * i + lon; - collision_with_margin.pose.position.y = y0 + y_step * i; - - lanelet::ArcCoordinates arc; - arc.length = obstacle_info.position.x; - arc.distance = obstacle_info.position.y; - - PossibleCollisionInfo col(obstacle_info, collision_with_margin, intersection_pose, arc); - possible_collisions.emplace_back(col); - } -} -inline void addConstantVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId & trajectory, double velocity) -{ - for (auto & p : trajectory.points) { - p.point.longitudinal_velocity_mps = velocity; - } -} -inline autoware_auto_perception_msgs::msg::PredictedObject generatePredictedObject(double x) -{ - autoware_auto_perception_msgs::msg::PredictedObject obj; - obj.shape.dimensions.x = 5.0; - obj.shape.dimensions.y = 2.0; - obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; - obj.kinematics.initial_pose_with_covariance.pose.position.x = x; - obj.kinematics.initial_pose_with_covariance.pose.position.y = 0; - obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); - obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - return obj; -} -inline geometry_msgs::msg::Pose generatePose(double x) -{ - geometry_msgs::msg::Pose p; - p.position.x = x; - p.position.y = 0.0; - p.position.z = 1.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0); - p.orientation = tf2::toMsg(q); - return p; -} - -} // namespace test - -#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..5de6cdd50cdf6 --- /dev/null +++ b/planning/behavior_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/utilization/path_utilization.cpp + src/utilization/util.cpp + src/utilization/debug.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_state_machine.cpp + test/src/test_arc_lane_util.cpp + test/src/test_utilization.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index ab702249c707c..44d3184a5f6b7 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #include "route_handler/route_handler.hpp" @@ -144,4 +144,4 @@ struct PlannerData }; } // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp new file mode 100644 index 0000000000000..e5606d7d10dff --- /dev/null +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 The Autoware Contributors +// +// 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_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ + +class PluginInterface +{ +public: + virtual ~PluginInterface() = default; + virtual void init(rclcpp::Node & node) = 0; + virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual boost::optional getFirstStopPathPointIndex() = 0; + virtual const char * getModuleName() = 0; +}; + +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp new file mode 100644 index 0000000000000..623649f82bf24 --- /dev/null +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 The Autoware Contributors +// +// 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_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ + +#include + +#include + +namespace behavior_velocity_planner +{ + +template +class PluginWrapper : public PluginInterface +{ +public: + void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } + void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override + { + scene_manager_->plan(path); + }; + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override + { + scene_manager_->updateSceneModuleInstances(planner_data, path); + } + boost::optional getFirstStopPathPointIndex() override + { + return scene_manager_->getFirstStopPathPointIndex(); + } + const char * getModuleName() override { return scene_manager_->getModuleName(); } + +private: + std::unique_ptr scene_manager_; +}; + +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 844cbc5f41d44..0b3dfd1213eb1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ -#define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ - -#include "behavior_velocity_planner/planner_data.hpp" -#include "velocity_factor_interface.hpp" +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#include +#include +#include #include #include #include -#include #include #include @@ -414,4 +413,4 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index dc282785f330d..ffc216e863500 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__ARC_LANE_UTIL_HPP_ -#define UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#include #include #include -#include #include @@ -293,4 +293,4 @@ inline boost::optional createTargetPoint( } // namespace arc_lane_utils } // namespace behavior_velocity_planner -#endif // UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 896f350abe9aa..193890ec70ceb 100644 --- a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -127,4 +127,4 @@ inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) } } // namespace behavior_velocity_planner -#endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp similarity index 87% rename from planning/behavior_velocity_planner/include/utilization/debug.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index af398ae539a1e..e840b6f8a5995 100644 --- a/planning/behavior_velocity_planner/include/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__DEBUG_HPP_ -#define UTILIZATION__DEBUG_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#include +#include #include @@ -44,4 +44,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const double r, const double g, const double b); } // namespace debug } // namespace behavior_velocity_planner -#endif // UTILIZATION__DEBUG_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 86% rename from planning/behavior_velocity_planner/include/utilization/path_utilization.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 7856a42114619..4156f7df9540b 100644 --- a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__PATH_UTILIZATION_HPP_ -#define UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include #include @@ -37,4 +37,4 @@ autoware_auto_planning_msgs::msg::Path filterStopPathPoint( const autoware_auto_planning_msgs::msg::Path & path); } // namespace behavior_velocity_planner -#endif // UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/utilization/state_machine.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp index b0b78d458e78f..73b0fa7d553a3 100644 --- a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__STATE_MACHINE_HPP_ -#define UTILIZATION__STATE_MACHINE_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -93,4 +93,4 @@ class StateMachine }; } // namespace behavior_velocity_planner -#endif // UTILIZATION__STATE_MACHINE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 92% rename from planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index ce04fe9d5c7a9..7d5278c3cf972 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include +#include #include #include #include #include -#include #include #include @@ -129,4 +129,4 @@ inline bool smoothPath( } // namespace behavior_velocity_planner -#endif // UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/utilization/util.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index f8cc7910ad649..14b009ceb0748 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTILIZATION__UTIL_HPP_ -#define UTILIZATION__UTIL_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#include #include #include #include -#include #include #include @@ -317,4 +317,4 @@ lanelet::ConstLanelets getConstLaneletsFromIds( } // namespace planning_utils } // namespace behavior_velocity_planner -#endif // UTILIZATION__UTIL_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp similarity index 90% rename from planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp rename to planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp index cff6e54b4f134..28ac40f0868a0 100644 --- a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ -#define SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ #include @@ -65,4 +65,4 @@ class VelocityFactorInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml new file mode 100644 index 0000000000000..293eee96dffdf --- /dev/null +++ b/planning/behavior_velocity_planner_common/package.xml @@ -0,0 +1,66 @@ + + + + behavior_velocity_planner_common + 0.1.0 + The behavior_velocity_planner_common package + + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + + Apache License 2.0 + + Yukihiro Saito + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + cv_bridge + diagnostic_msgs + eigen + geometry_msgs + grid_map_cv + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + message_filters + motion_utils + motion_velocity_smoother + nav_msgs + nlohmann-json-dev + pcl_conversions + rclcpp + rclcpp_components + route_handler + rtc_interface + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_api_msgs + tier4_autoware_utils + tier4_planning_msgs + tier4_v2x_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/utilization/debug.cpp rename to planning/behavior_velocity_planner_common/src/utilization/debug.cpp index ec3bfef1fa2de..dc1eaeacb2f71 100644 --- a/planning/behavior_velocity_planner/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/utilization/path_utilization.cpp rename to planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index d570492fd93b3..34d4cf0efa7ac 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/utilization/util.cpp rename to planning/behavior_velocity_planner_common/src/utilization/util.cpp index d3d73927c2727..42058d2e16316 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp similarity index 98% rename from planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp rename to planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index ac5a5c82d6730..c4fdb83e00609 100644 --- a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" -#include -#include +#include +#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp similarity index 97% rename from planning/behavior_velocity_planner/test/src/test_state_machine.cpp rename to planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp index 34a6b5b0767b4..ac7c6655ced87 100644 --- a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" +#include #include -#include #include diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp similarity index 96% rename from planning/behavior_velocity_planner/test/src/test_utilization.cpp rename to planning/behavior_velocity_planner_common/test/src/test_utilization.cpp index 742cccecb9773..f43450800b46e 100644 --- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include "motion_utils/trajectory/trajectory.hpp" -#include "utilization/path_utilization.hpp" #include "utils.hpp" -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner_common/test/src/utils.hpp new file mode 100644 index 0000000000000..1328edb6f6027 --- /dev/null +++ b/planning/behavior_velocity_planner_common/test/src/utils.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include + +#include + +namespace test +{ + +inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +inline geometry_msgs::msg::Pose generatePose(double x) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = 0.0; + p.position.z = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.orientation = tf2::toMsg(q); + return p; +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt new file mode 100644 index 0000000000000..a644d7515c692 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_run_out_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/dynamic_obstacle.cpp + src/manager.cpp + src/scene.cpp + src/state_machine.cpp + src/utils.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml new file mode 100644 index 0000000000000..fb3c49bb750a6 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -0,0 +1,46 @@ + + + + behavior_velocity_run_out_module + 0.1.0 + The behavior_velocity_run_out_module package + + Tomohito Ando + Makoto Kurihara + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Tomohito Ando + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + libboost-dev + message_filters + motion_utils + pcl_conversions + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tf2_eigen + tf2_ros + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml new file mode 100644 index 0000000000000..5320d46f92929 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp rename to planning/behavior_velocity_run_out_module/src/debug.cpp index 76b5434b2af49..f47baae67f714 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/debug.hpp" +#include "debug.hpp" -#include "scene_module/run_out/scene.hpp" +#include "scene.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp similarity index 96% rename from planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp rename to planning/behavior_velocity_run_out_module/src/debug.hpp index 37f9399d6a525..e281f323f56cb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -11,10 +11,10 @@ // 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 SCENE_MODULE__RUN_OUT__DEBUG_HPP_ -#define SCENE_MODULE__RUN_OUT__DEBUG_HPP_ +#ifndef DEBUG_HPP_ +#define DEBUG_HPP_ -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" #include #include @@ -145,4 +145,4 @@ class RunOutDebug } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__DEBUG_HPP_ +#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp rename to planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 290cf9af97dd0..b4dce723de5d5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/dynamic_obstacle.hpp" +#include "dynamic_obstacle.hpp" #include +#include +#include + namespace behavior_velocity_planner { namespace diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp similarity index 93% rename from planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp rename to planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 7a4fa3377e4d2..f9b2b12298d2d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ -#define SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ +#ifndef DYNAMIC_OBSTACLE_HPP_ +#define DYNAMIC_OBSTACLE_HPP_ -#include "behavior_velocity_planner/planner_data.hpp" -#include "scene_module/run_out/debug.hpp" -#include "scene_module/run_out/utils.hpp" -#include "utilization/path_utilization.hpp" -#include "utilization/util.hpp" +#include "debug.hpp" +#include "utils.hpp" +#include +#include +#include #include #include @@ -171,4 +171,4 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ +#endif // DYNAMIC_OBSTACLE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp rename to planning/behavior_velocity_run_out_module/src/manager.cpp index 884140000e74c..8887f0749a2d5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/manager.hpp" +#include "manager.hpp" + +#include +#include namespace behavior_velocity_planner { @@ -182,3 +185,7 @@ void RunOutModuleManager::setDynamicObstacleCreator( } } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp similarity index 79% rename from planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp rename to planning/behavior_velocity_run_out_module/src/manager.hpp index ea31cf198fa30..4dd66ad45898b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__MANAGER_HPP_ -#define SCENE_MODULE__RUN_OUT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ -#include "scene_module/run_out/scene.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "scene.hpp" + +#include +#include +#include #include @@ -41,6 +44,11 @@ class RunOutModuleManager : public SceneModuleManagerInterface void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; + +class RunOutModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp similarity index 90% rename from planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp rename to planning/behavior_velocity_run_out_module/src/path_utils.hpp index 50a0f8728f0b3..b01ee70f8bdfa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ -#define SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ +#ifndef PATH_UTILS_HPP_ +#define PATH_UTILS_HPP_ #include @@ -50,4 +50,4 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ +#endif // PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp rename to planning/behavior_velocity_run_out_module/src/scene.cpp index ee8c2e5d0ab75..3b2ed6a78a96c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -12,11 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/scene.hpp" +#include "scene.hpp" -#include "scene_module/run_out/path_utils.hpp" -#include "utilization/trajectory_utils.hpp" -#include "utilization/util.hpp" +#include "path_utils.hpp" + +#include +#include + +#include +#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp rename to planning/behavior_velocity_run_out_module/src/scene.hpp index 3a04065f52c75..017ed0cb17c50 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__SCENE_HPP_ -#define SCENE_MODULE__RUN_OUT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ -#include "scene_module/run_out/debug.hpp" -#include "scene_module/run_out/dynamic_obstacle.hpp" -#include "scene_module/run_out/state_machine.hpp" -#include "scene_module/run_out/utils.hpp" -#include "scene_module/scene_module_interface.hpp" +#include "debug.hpp" +#include "dynamic_obstacle.hpp" +#include "state_machine.hpp" +#include "utils.hpp" + +#include #include #include @@ -143,4 +144,4 @@ class RunOutModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp b/planning/behavior_velocity_run_out_module/src/state_machine.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp rename to planning/behavior_velocity_run_out_module/src/state_machine.cpp index daeb86393b1e9..322264cad8501 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp +++ b/planning/behavior_velocity_run_out_module/src/state_machine.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/state_machine.hpp" +#include "state_machine.hpp" namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp b/planning/behavior_velocity_run_out_module/src/state_machine.hpp similarity index 89% rename from planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp rename to planning/behavior_velocity_run_out_module/src/state_machine.hpp index 0ee34bf6c9685..d3339fa0ef129 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp +++ b/planning/behavior_velocity_run_out_module/src/state_machine.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ -#define SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ +#ifndef STATE_MACHINE_HPP_ +#define STATE_MACHINE_HPP_ -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" #include #include @@ -59,4 +59,4 @@ class StateMachine } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__STATE_MACHINE_HPP_ +#endif // STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp rename to planning/behavior_velocity_run_out_module/src/utils.cpp index 2093d0a01f453..3def2ade46731 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/run_out/utils.hpp" +#include "utils.hpp" + +#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp similarity index 97% rename from planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp rename to planning/behavior_velocity_run_out_module/src/utils.hpp index 39f41e9da11ff..1f725291a38b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__RUN_OUT__UTILS_HPP_ -#define SCENE_MODULE__RUN_OUT__UTILS_HPP_ - -#include "behavior_velocity_planner/planner_data.hpp" -#include "utilization/util.hpp" +#ifndef UTILS_HPP_ +#define UTILS_HPP_ +#include +#include #include #include @@ -243,4 +242,4 @@ Polygons2d createMandatoryDetectionAreaPolygon( const PlannerParam & planner_param); } // namespace run_out_utils } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__RUN_OUT__UTILS_HPP_ +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/CMakeLists.txt b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt new file mode 100644 index 0000000000000..0d79afda4d412 --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_speed_bump_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 + src/util.cpp +) + +ament_auto_package() diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml new file mode 100644 index 0000000000000..2f10b111c043e --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_speed_bump_module + 0.1.0 + The behavior_velocity_speed_bump_module package + + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml new file mode 100644 index 0000000000000..506d25669f8cf --- /dev/null +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp rename to planning/behavior_velocity_speed_bump_module/src/debug.cpp index a51a14f747f4b..6e92672eb51f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/scene.hpp" +#include "scene.hpp" +#include #include #include -#include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp rename to planning/behavior_velocity_speed_bump_module/src/manager.cpp index f6534aa1610aa..e502963e60b47 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/manager.hpp" +#include "manager.hpp" #include @@ -78,3 +78,7 @@ SpeedBumpModuleManager::getModuleExpiredFunction( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp rename to planning/behavior_velocity_speed_bump_module/src/manager.hpp index d64870f6a95d4..480208067d87e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/manager.hpp +++ b/planning/behavior_velocity_speed_bump_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__SPEED_BUMP__MANAGER_HPP_ -#define SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -41,6 +44,11 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class SpeedBumpModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp rename to planning/behavior_velocity_speed_bump_module/src/scene.cpp index 49506e5bcc8f2..fda92f037cf7a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scene_module/speed_bump/scene.hpp" +#include "scene.hpp" #include "motion_utils/motion_utils.hpp" -#include "scene_module/speed_bump/util.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "util.hpp" #include diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp rename to planning/behavior_velocity_speed_bump_module/src/scene.hpp index 74f9da149386e..bf8baf92dae00 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ -#define SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ +#include "util.hpp" + +#include #include -#include -#include #include #include @@ -81,4 +82,4 @@ class SpeedBumpModule : public SceneModuleInterface } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp rename to planning/behavior_velocity_speed_bump_module/src/util.cpp index 7a8bd9edfd515..89845a4b5e271 100644 --- a/planning/behavior_velocity_planner/src/scene_module/speed_bump/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "util.hpp" + #include "motion_utils/motion_utils.hpp" -#include +#include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp rename to planning/behavior_velocity_speed_bump_module/src/util.hpp index 2aafee4a113ab..5b23661431001 100644 --- a/planning/behavior_velocity_planner/include/scene_module/speed_bump/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ -#define SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ +#ifndef UTIL_HPP_ +#define UTIL_HPP_ #include #include @@ -30,6 +30,7 @@ #include #include +#include #include @@ -70,4 +71,4 @@ float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float spee } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__SPEED_BUMP__UTIL_HPP_ +#endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_stop_line_module/CMakeLists.txt new file mode 100644 index 0000000000000..b32a0b6f2f3b7 --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_stop_line_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_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml new file mode 100644 index 0000000000000..8610b7b428bec --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_velocity_stop_line_module + 0.1.0 + The behavior_velocity_stop_line_module package + + Yutaka Shimizu + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Yutaka Shimizu + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + motion_utils + pluginlib + rclcpp + route_handler + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml new file mode 100644 index 0000000000000..51fb225fbebad --- /dev/null +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp rename to planning/behavior_velocity_stop_line_module/src/debug.cpp index feb450358e3ee..d2b9527b1dd7d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp rename to planning/behavior_velocity_stop_line_module/src/manager.cpp index 0b647c69dd3d2..86db87ad602cc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" #include #include @@ -100,4 +100,9 @@ StopLineModuleManager::getModuleExpiredFunction( return stop_line_id_set.count(scene_module->getModuleId()) == 0; }; } + } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp similarity index 83% rename from planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp rename to planning/behavior_velocity_stop_line_module/src/manager.hpp index 68ad08594a70f..4b665a3a536f7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp +++ b/planning/behavior_velocity_stop_line_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__STOP_LINE__MANAGER_HPP_ -#define SCENE_MODULE__STOP_LINE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -54,6 +57,11 @@ class StopLineModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class StopLineModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__STOP_LINE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_stop_line_module/src/scene.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp rename to planning/behavior_velocity_stop_line_module/src/scene.cpp index 96e306df62fc0..a703f8b5566ad 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp similarity index 91% rename from planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp rename to planning/behavior_velocity_stop_line_module/src/scene.hpp index f9846f970beb5..f7540901a5f80 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__STOP_LINE__SCENE_HPP_ -#define SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include #include @@ -24,11 +24,11 @@ #define EIGEN_MPL2_ONLY #include #include +#include +#include +#include #include #include -#include -#include -#include #include #include @@ -111,4 +111,4 @@ class StopLineModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__STOP_LINE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt new file mode 100644 index 0000000000000..97a8922009dc0 --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_traffic_light_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_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml new file mode 100644 index 0000000000000..a8733c5dcf5be --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -0,0 +1,45 @@ + + + + behavior_velocity_traffic_light_module + 0.1.0 + The behavior_velocity_traffic_light_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..b65cc66c5c232 --- /dev/null +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp rename to planning/behavior_velocity_traffic_light_module/src/debug.cpp index 20363f8633cd0..a57cfe68f1f59 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp rename to planning/behavior_velocity_traffic_light_module/src/manager.cpp index 551fdb6e59425..cad2ed3bda069 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" #include +#include #include #include #include @@ -202,3 +203,7 @@ bool TrafficLightModuleManager::hasSameTrafficLight( } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp similarity index 83% rename from planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp rename to planning/behavior_velocity_traffic_light_module/src/manager.hpp index b93205e5acdb8..450cba09e1e08 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_traffic_light_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__TRAFFIC_LIGHT__MANAGER_HPP_ -#define SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -57,6 +60,11 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC boost::optional first_ref_stop_path_point_index_; }; + +class TrafficLightModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp rename to planning/behavior_velocity_traffic_light_module/src/scene.cpp index b7d345e9d2886..925dfb0470b98 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include #include -#include -#include #include // To be replaced by std::optional in C++17 @@ -27,6 +28,7 @@ #endif #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp similarity index 94% rename from planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp rename to planning/behavior_velocity_traffic_light_module/src/scene.hpp index 47b2f8b74d94e..b96c6ebcbb297 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ -#define SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include #include @@ -24,10 +24,10 @@ #define EIGEN_MPL2_ONLY #include #include +#include +#include #include #include -#include -#include #include @@ -148,4 +148,4 @@ class TrafficLightModule : public SceneModuleInterface }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt new file mode 100644 index 0000000000000..b54fde0b0ce6a --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_virtual_traffic_light_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_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_virtual_traffic_light_module/package.xml new file mode 100644 index 0000000000000..06b4e7318a791 --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/package.xml @@ -0,0 +1,42 @@ + + + + behavior_velocity_virtual_traffic_light_module + 0.1.0 + The behavior_velocity_virtual_traffic_light_module package + + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_planning_msgs + behavior_velocity_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + nlohmann-json-dev + pluginlib + rclcpp + route_handler + tier4_autoware_utils + tier4_planning_msgs + tier4_v2x_msgs + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..943ef175aa8f8 --- /dev/null +++ b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index f20c7b9125ffb..78c22f8873a09 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + #include -#include #include using motion_utils::createStopVirtualWallMarker; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp similarity index 93% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 732e5eb0cb3c7..e5a2b0e43def2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "manager.hpp" + #include #include @@ -72,3 +73,8 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( }; } } // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::VirtualTrafficLightModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp similarity index 78% rename from planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp index b844bc68ab260..15bd6f468132e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_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__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ -#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ +#include "scene.hpp" + +#include +#include +#include #include -#include -#include #include @@ -40,6 +43,11 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; + +class VirtualTrafficLightModulePlugin : public PluginWrapper +{ +}; + } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp index affc5d7ef8259..fd71635873e7f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "scene.hpp" + +#include +#include #include -#include -#include -#include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp similarity index 95% rename from planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp rename to planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 2bd7078d93aeb..bd005fced0a27 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ -#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ +#include #include #include #include #include #include -#include #include #include @@ -130,4 +130,4 @@ class VirtualTrafficLightModule : public SceneModuleInterface tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; } // namespace behavior_velocity_planner -#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ +#endif // SCENE_HPP_