diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 1d1ad56342f4d..53e2427228b72 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -275,6 +275,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishObjectsOfInterestMarker(); + processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); return result; @@ -298,6 +300,7 @@ class PlannerManager { module_ptr->onExit(); module_ptr->publishRTCStatus(); + module_ptr->publishObjectsOfInterestMarker(); module_ptr.reset(); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index cbf40e59535be..76264b0532f07 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -49,7 +49,9 @@ class AvoidanceModule : public SceneModuleInterface public: AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index 9d89138f7cdfe..b2defad526e8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -37,7 +37,9 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index a12cc4d405d41..37bd28576928c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -268,7 +268,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index 10a0d055a5a77..1a9088f2226ff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -37,7 +37,8 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface std::unique_ptr createNewSceneModuleInstance() override { return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_); + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 97a5fdd966b8f..25f0a815dee8c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -261,7 +261,9 @@ class GoalPlannerModule : public SceneModuleInterface GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index 4a7eecfc68caf..11ce4e0c3e94c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -35,7 +35,9 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index f2427f185fbaa..dad46494bc350 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -25,7 +25,6 @@ #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" #include @@ -49,8 +48,6 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using objects_of_interest_marker_interface::ColorName; -using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -59,7 +56,9 @@ class LaneChangeInterface : public SceneModuleInterface public: LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; @@ -136,13 +135,6 @@ class LaneChangeInterface : public SceneModuleInterface void setObjectDebugVisualization() const; - void setObjectsOfInterestData(const bool is_approved); - - void publishObjectsOfInterestData() - { - objects_of_interest_marker_interface_.publishMarkerArray(); - } - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( @@ -152,7 +144,6 @@ class LaneChangeInterface : public SceneModuleInterface mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; std::unique_ptr prev_approved_path_; - ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; void clearAbortApproval() { is_abort_path_approved_ = false; } @@ -168,7 +159,9 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 534594d93445c..df71f0577964a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,8 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; @@ -81,11 +84,15 @@ class SceneModuleInterface public: SceneModuleInterface( const std::string & name, rclcpp::Node & node, - std::unordered_map> rtc_interface_ptr_map) + std::unordered_map> rtc_interface_ptr_map, + std::unordered_map> + objects_of_interest_marker_interface_ptr_map) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), + objects_of_interest_marker_interface_ptr_map_( + std::move(objects_of_interest_marker_interface_ptr_map)), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { @@ -191,6 +198,15 @@ class SceneModuleInterface } } + void publishObjectsOfInterestMarker() + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->publishMarkerArray(); + } + } + } + void publishSteeringFactor() { if (!steering_factor_interface_ptr_) { @@ -415,6 +431,17 @@ class SceneModuleInterface } } + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->insertObjectData(obj_pose, obj_shape, color_name); + } + } + } + /** * @brief Return SUCCESS if plan is not needed or plan is successfully finished, * FAILURE if plan has failed, RUNNING if plan is on going. @@ -569,6 +596,9 @@ class SceneModuleInterface std::unordered_map> rtc_interface_ptr_map_; + std::unordered_map> + objects_of_interest_marker_interface_ptr_map_; + std::unique_ptr steering_factor_interface_ptr_; mutable boost::optional stop_pose_{boost::none}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 38101416a80b3..849fa2666894d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -65,6 +65,8 @@ class SceneModuleManagerInterface rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; rtc_interface_ptr_map_.emplace( rtc_type, std::make_shared(node, rtc_interface_name, enable_rtc_)); + objects_of_interest_marker_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); } pub_info_marker_ = node->create_publisher("~/info/" + name, 20); @@ -305,6 +307,9 @@ class SceneModuleManagerInterface std::unordered_map> rtc_interface_ptr_map_; + std::unordered_map> + objects_of_interest_marker_interface_ptr_map_; + bool enable_simultaneous_execution_as_approved_module_{false}; bool enable_simultaneous_execution_as_candidate_module_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index b74be38c0faf9..0d58fc1abe240 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -36,7 +36,9 @@ class SideShiftModuleManager : public SceneModuleManagerInterface std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index fce31b6db7369..8e2419a334352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -43,7 +43,9 @@ class SideShiftModule : public SceneModuleInterface SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 3ee376f4111f0..6cc3b6d7c1c46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -35,7 +35,9 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 54e13de85e8da..bc5084cacbe2b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -79,7 +79,9 @@ class StartPlannerModule : public SceneModuleInterface StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index a3cb4dd6dc8e2..ee2f1160d76c2 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -151,8 +151,10 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, helper_{parameters} { diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 3aa95dba6c4ad..7bc61a4d747c7 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -248,8 +248,10 @@ double calcDistanceToSegment( DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b2692200557ed..8291624f943da 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -54,8 +54,10 @@ namespace behavior_path_planner GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_} diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 57acfa5002296..ca1b2384051f6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -36,12 +36,13 @@ using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()}, - objects_of_interest_marker_interface_{&node, name} + prev_approved_path_{std::make_unique()} { steering_factor_interface_ptr_ = std::make_unique(&node, name); } @@ -199,8 +200,10 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); - setObjectsOfInterestData(true); - publishObjectsOfInterestData(); + for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -224,8 +227,11 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); setObjectDebugVisualization(); - setObjectsOfInterestData(false); - publishObjectsOfInterestData(); + + for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); @@ -314,18 +320,6 @@ void LaneChangeInterface::setObjectDebugVisualization() const } } -void LaneChangeInterface::setObjectsOfInterestData(const bool is_approved) -{ - const auto debug_data = - is_approved ? module_type_->getAfterApprovalDebugData() : module_type_->getDebugData(); - for (const auto & [uuid, data] : debug_data) { - const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; - objects_of_interest_marker_interface_.insertObjectData( - data.current_obj_pose, data.obj_shape, color); - } - return; -} - std::shared_ptr LaneChangeInterface::get_debug_msg_array() const { const auto debug_data = module_type_->getDebugData(); @@ -509,9 +503,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) : LaneChangeInterface{ - name, node, parameters, rtc_interface_ptr_map, + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index e772c21331ae8..3a0b7527e4184 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -221,10 +221,12 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod if (type_ == LaneChangeModuleType::NORMAL) { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, std::make_unique(parameters_, direction_)); } @@ -371,7 +373,8 @@ std::unique_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( - name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 0e86508ca6afc..64ae5d3f26c2c 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -38,8 +38,11 @@ using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + parameters_{parameters} { } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 06b195ea29b50..f847a8be7df2b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -44,8 +44,10 @@ namespace behavior_path_planner StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} {