Skip to content

Commit c6788cf

Browse files
satoshi-otasatoshi-ota
authored
refactor(bpp): use pluginlib to load scene module (#5771)
* refactor(bpp): use pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(tier4_planning_launch): update launcher Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(avoidance): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(lane_change): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(dynamic_avoidance): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(goal_planner): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(side_shift): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(start_planner): support pluginlib Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(bpp): move interface Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(bpp): add const Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent 04aa54b commit c6788cf

40 files changed

+509
-425
lines changed

‎launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

+66-10
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,17 @@
1616
<arg name="launch_start_planner_module" default="true"/>
1717
<arg name="launch_side_shift_module" default="true"/>
1818

19+
<arg name="launch_avoidance_module" default="true"/>
20+
<arg name="launch_dynamic_avoidance_module" default="true"/>
21+
<arg name="launch_side_shift_module" default="true"/>
22+
<arg name="launch_goal_planner_module" default="true"/>
23+
<arg name="launch_start_planner_module" default="true"/>
24+
<arg name="launch_lane_change_right_module" default="true"/>
25+
<arg name="launch_lane_change_left_module" default="true"/>
26+
<arg name="launch_external_request_lane_change_right_module" default="true"/>
27+
<arg name="launch_external_request_lane_change_left_module" default="true"/>
28+
<arg name="launch_avoidance_by_lane_change_module" default="true"/>
29+
1930
<arg name="launch_crosswalk_module" default="true"/>
2031
<arg name="launch_walkway_module" default="true"/>
2132
<arg name="launch_traffic_light_module" default="true"/>
@@ -32,6 +43,60 @@
3243
<arg name="launch_out_of_lane_module" default="true"/>
3344
<arg name="launch_no_drivable_lane_module" default="true"/>
3445

46+
<!-- assemble launch config for behavior path planner -->
47+
<arg name="behavior_path_planner_launch_modules" default="["/>
48+
<let
49+
name="behavior_path_planner_launch_modules"
50+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::AvoidanceModuleManager, '&quot;)"
51+
if="$(var launch_avoidance_module)"
52+
/>
53+
<let
54+
name="behavior_path_planner_launch_modules"
55+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::DynamicAvoidanceModuleManager, '&quot;)"
56+
if="$(var launch_dynamic_avoidance_module)"
57+
/>
58+
<let
59+
name="behavior_path_planner_launch_modules"
60+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::SideShiftModuleManager, '&quot;)"
61+
if="$(var launch_side_shift_module)"
62+
/>
63+
<let
64+
name="behavior_path_planner_launch_modules"
65+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::StartPlannerModuleManager, '&quot;)"
66+
if="$(var launch_start_planner_module)"
67+
/>
68+
<let
69+
name="behavior_path_planner_launch_modules"
70+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::GoalPlannerModuleManager, '&quot;)"
71+
if="$(var launch_goal_planner_module)"
72+
/>
73+
<let
74+
name="behavior_path_planner_launch_modules"
75+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::LaneChangeRightModuleManager, '&quot;)"
76+
if="$(var launch_lane_change_right_module)"
77+
/>
78+
<let
79+
name="behavior_path_planner_launch_modules"
80+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::LaneChangeLeftModuleManager, '&quot;)"
81+
if="$(var launch_lane_change_left_module)"
82+
/>
83+
<let
84+
name="behavior_path_planner_launch_modules"
85+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, '&quot;)"
86+
if="$(var launch_external_request_lane_change_right_module)"
87+
/>
88+
<let
89+
name="behavior_path_planner_launch_modules"
90+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, '&quot;)"
91+
if="$(var launch_external_request_lane_change_left_module)"
92+
/>
93+
<let
94+
name="behavior_path_planner_launch_modules"
95+
value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + 'behavior_path_planner::AvoidanceByLaneChangeModuleManager, '&quot;)"
96+
if="$(var launch_avoidance_by_lane_change_module)"
97+
/>
98+
<let name="behavior_path_planner_launch_modules" value="$(eval &quot;'$(var behavior_path_planner_launch_modules)' + ']'&quot;)"/>
99+
35100
<!-- assemble launch config for behavior velocity planner -->
36101
<arg name="behavior_velocity_planner_launch_modules" default="["/>
37102
<let
@@ -129,19 +194,10 @@
129194
<remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
130195
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
131196
<!-- params -->
132-
<param name="avoidance.enable_module" value="$(var launch_avoidance_module)"/>
133-
<param name="avoidance_by_lc.enable_module" value="$(var launch_avoidance_by_lane_change_module)"/>
134-
<param name="dynamic_avoidance.enable_module" value="$(var launch_dynamic_avoidance_module)"/>
135-
<param name="lane_change_right.enable_module" value="$(var launch_lane_change_right_module)"/>
136-
<param name="lane_change_left.enable_module" value="$(var launch_lane_change_left_module)"/>
137-
<param name="external_request_lane_change_left.enable_module" value="$(var launch_external_request_lane_change_left_module)"/>
138-
<param name="external_request_lane_change_right.enable_module" value="$(var launch_external_request_lane_change_right_module)"/>
139-
<param name="goal_planner.enable_module" value="$(var launch_goal_planner_module)"/>
140-
<param name="start_planner.enable_module" value="$(var launch_start_planner_module)"/>
141-
<param name="side_shift.enable_module" value="$(var launch_side_shift_module)"/>
142197
<param from="$(var common_param_path)"/>
143198
<param from="$(var vehicle_param_file)"/>
144199
<param from="$(var nearest_search_param_path)"/>
200+
<param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
145201
<param from="$(var behavior_path_planner_side_shift_module_param_path)"/>
146202
<param from="$(var behavior_path_planner_avoidance_module_param_path)"/>
147203
<param from="$(var behavior_path_planner_avoidance_by_lc_module_param_path)"/>

‎planning/behavior_path_planner/CMakeLists.txt

+12-11
Original file line numberDiff line numberDiff line change
@@ -7,11 +7,11 @@ autoware_package()
77
find_package(OpenCV REQUIRED)
88
find_package(magic_enum CONFIG REQUIRED)
99

10-
ament_auto_add_library(behavior_path_planner_node SHARED
10+
pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml)
11+
12+
ament_auto_add_library(${PROJECT_NAME}_lib SHARED
1113
src/planner_manager.cpp
12-
src/steering_factor_interface.cpp
1314
src/behavior_path_planner_node.cpp
14-
src/scene_module/scene_module_visitor.cpp
1515
src/scene_module/avoidance/avoidance_module.cpp
1616
src/scene_module/avoidance/manager.cpp
1717
src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -48,17 +48,17 @@ ament_auto_add_library(behavior_path_planner_node SHARED
4848
src/marker_utils/lane_change/debug.cpp
4949
)
5050

51-
target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
51+
target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC
5252
${EIGEN3_INCLUDE_DIR}
5353
)
5454

55-
target_link_libraries(behavior_path_planner_node
55+
target_link_libraries(${PROJECT_NAME}_lib
5656
${OpenCV_LIBRARIES}
5757
)
5858

59-
rclcpp_components_register_node(behavior_path_planner_node
59+
rclcpp_components_register_node(${PROJECT_NAME}_lib
6060
PLUGIN "behavior_path_planner::BehaviorPathPlannerNode"
61-
EXECUTABLE behavior_path_planner
61+
EXECUTABLE ${PROJECT_NAME}_node
6262
)
6363

6464
if(BUILD_TESTING)
@@ -68,30 +68,31 @@ if(BUILD_TESTING)
6868
)
6969

7070
target_link_libraries(test_${CMAKE_PROJECT_NAME}_utilities
71-
behavior_path_planner_node
71+
${PROJECT_NAME}_lib
7272
)
7373

7474
ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_avoidance_module
7575
test/test_avoidance_utils.cpp
7676
)
7777

7878
target_link_libraries(test_${CMAKE_PROJECT_NAME}_avoidance_module
79-
behavior_path_planner_node
79+
${PROJECT_NAME}_lib
8080
)
8181

8282
ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module
8383
test/test_lane_change_utils.cpp
8484
)
8585

8686
target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module
87-
behavior_path_planner_node
87+
${PROJECT_NAME}_lib
8888
)
8989

9090
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface
9191
test/test_${PROJECT_NAME}_node_interface.cpp
9292
)
93+
9394
target_link_libraries(test_${PROJECT_NAME}_node_interface
94-
behavior_path_planner_node
95+
${PROJECT_NAME}_lib
9596
)
9697
endif()
9798

‎planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,9 @@
1616
#define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_
1717

1818
#include "behavior_path_planner/planner_manager.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
20-
#include "behavior_path_planner/steering_factor_interface.hpp"
2119
#include "behavior_path_planner_common/data_manager.hpp"
20+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
21+
#include "behavior_path_planner_common/interface/steering_factor_interface.hpp"
2222
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
2323

2424
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>

‎planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

+16-10
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,13 @@
1515
#ifndef BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_
1616
#define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_
1717

18-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
20-
#include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
18+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
19+
#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
20+
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
2121
#include "tier4_autoware_utils/ros/debug_publisher.hpp"
2222
#include "tier4_autoware_utils/system/stop_watch.hpp"
2323

24+
#include <pluginlib/class_loader.hpp>
2425
#include <rclcpp/rclcpp.hpp>
2526

2627
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -104,14 +105,17 @@ class PlannerManager
104105

105106
/**
106107
* @brief register managers.
107-
* @param manager pointer.
108+
* @param node.
109+
* @param plugin name.
108110
*/
109-
void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr)
110-
{
111-
RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str());
112-
manager_ptrs_.push_back(manager_ptr);
113-
processing_time_.emplace(manager_ptr->name(), 0.0);
114-
}
111+
void launchScenePlugin(rclcpp::Node & node, const std::string & name);
112+
113+
/**
114+
* @brief unregister managers.
115+
* @param node.
116+
* @param plugin name.
117+
*/
118+
void removeScenePlugin(rclcpp::Node & node, const std::string & name);
115119

116120
/**
117121
* @brief update module parameters. used for dynamic reconfigure.
@@ -434,6 +438,8 @@ class PlannerManager
434438

435439
std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
436440

441+
pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
442+
437443
mutable rclcpp::Logger logger_;
438444

439445
mutable rclcpp::Clock clock_;

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,11 @@
1515
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_
1717

18-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
2018
#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
2119
#include "behavior_path_planner/utils/avoidance/helper.hpp"
2220
#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp"
21+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
22+
#include "behavior_path_planner_common/interface/scene_module_visitor.hpp"
2323
#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
2424

2525
#include <rclcpp/rclcpp.hpp>

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_
1717

1818
#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
2019
#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp"
20+
#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
2121

2222
#include <rclcpp/rclcpp.hpp>
2323

@@ -32,8 +32,9 @@ namespace behavior_path_planner
3232
class AvoidanceModuleManager : public SceneModuleManagerInterface
3333
{
3434
public:
35-
AvoidanceModuleManager(
36-
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
35+
AvoidanceModuleManager() : SceneModuleManagerInterface{"avoidance"} {}
36+
37+
void init(rclcpp::Node * node) override;
3738

3839
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
3940
{

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_
1717

18-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
18+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
1919

2020
#include <rclcpp/rclcpp.hpp>
2121
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_
1717

1818
#include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
19+
#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
2020

2121
#include <rclcpp/rclcpp.hpp>
2222

@@ -31,8 +31,9 @@ namespace behavior_path_planner
3131
class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
3232
{
3333
public:
34-
DynamicAvoidanceModuleManager(
35-
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
34+
DynamicAvoidanceModuleManager() : SceneModuleManagerInterface{"dynamic_avoidance"} {}
35+
36+
void init(rclcpp::Node * node) override;
3637

3738
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
3839
{

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_
1717

18-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
1918
#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
2019
#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp"
2120
#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp"
@@ -25,6 +24,7 @@
2524
#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp"
2625
#include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
2726
#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp"
27+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
2828
#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
2929
#include "behavior_path_planner_common/utils/utils.hpp"
3030

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_
1717

1818
#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp"
19-
#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
19+
#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp"
2020

2121
#include <rclcpp/rclcpp.hpp>
2222

@@ -30,8 +30,9 @@ namespace behavior_path_planner
3030
class GoalPlannerModuleManager : public SceneModuleManagerInterface
3131
{
3232
public:
33-
GoalPlannerModuleManager(
34-
rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
33+
GoalPlannerModuleManager() : SceneModuleManagerInterface{"goal_planner"} {}
34+
35+
void init(rclcpp::Node * node) override;
3536

3637
std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
3738
{

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,10 @@
1414
#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
1515
#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
1616

17-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
1817
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
1918
#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
2019
#include "behavior_path_planner/utils/lane_change/utils.hpp"
20+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
2121
#include "behavior_path_planner_common/turn_signal_decider.hpp"
2222
#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
2323
#include "tier4_autoware_utils/system/stop_watch.hpp"

‎planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,9 @@
1919
#include "behavior_path_planner/scene_module/lane_change/base_class.hpp"
2020
#include "behavior_path_planner/scene_module/lane_change/external_request.hpp"
2121
#include "behavior_path_planner/scene_module/lane_change/normal.hpp"
22-
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
2322
#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
2423
#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
24+
#include "behavior_path_planner_common/interface/scene_module_interface.hpp"
2525
#include "behavior_path_planner_common/turn_signal_decider.hpp"
2626
#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
2727

0 commit comments

Comments
 (0)