diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 62a95ada62b09..18bf12fa8cdb8 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -34,7 +34,7 @@ The successive shortest path algorithm is used to solve the data association pro | `max_rad_matrix` | double | Maximum angle table for data association | | `base_link_frame_id` | double | association frame | | `distance_threshold_list` | `std::vector` | Distance threshold for each class used in judging overlap. The class order depends on [ObjectClassification](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/ObjectClassification.idl). | -| `generalized_iou_threshold` | double | Generalized IoU threshold | +| `generalized_iou_threshold` | `std::vector` | Generalized IoU threshold for each class | ## Tips diff --git a/perception/object_merger/config/overlapped_judge.param.yaml b/perception/object_merger/config/overlapped_judge.param.yaml index 94882fae4f282..0410b77390187 100644 --- a/perception/object_merger/config/overlapped_judge.param.yaml +++ b/perception/object_merger/config/overlapped_judge.param.yaml @@ -3,3 +3,6 @@ distance_threshold_list: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN + + generalized_iou_threshold: + [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 704f7859f6672..1e5b9fad9c9ca 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -76,7 +76,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node { double precision_threshold; double recall_threshold; - double generalized_iou_threshold; + std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; }; diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 1b89693b0f9f7..3418f7d5a5e61 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -14,7 +14,7 @@ - + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 17397b782892b..4f600ce8a4948 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -35,8 +35,11 @@ bool isUnknownObjectOverlapped( const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object, const autoware_auto_perception_msgs::msg::DetectedObject & known_object, const double precision_threshold, const double recall_threshold, - std::map distance_threshold_map, const double generalized_iou_threshold) + std::map distance_threshold_map, + const std::map generalized_iou_threshold_map) { + const double generalized_iou_threshold = generalized_iou_threshold_map.at( + object_recognition_utils::getHighestProbLabel(known_object.classification)); const double distance_threshold = distance_threshold_map.at( object_recognition_utils::getHighestProbLabel(known_object.classification)); const double sq_distance_threshold = std::pow(distance_threshold, 2.0); @@ -95,7 +98,7 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio overlapped_judge_param_.recall_threshold = declare_parameter("recall_threshold_to_judge_overlapped", 0.5); overlapped_judge_param_.generalized_iou_threshold = - declare_parameter("generalized_iou_threshold"); + convertListToClassMap(declare_parameter>("generalized_iou_threshold")); // get distance_threshold_map from distance_threshold_list /** TODO(Shin-kyoto): 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 8820ef70c37bf..02e5452cc2103 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 @@ -105,9 +105,9 @@ class PlannerManager */ void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) { - RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str()); + RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str()); manager_ptrs_.push_back(manager_ptr); - processing_time_.emplace(manager_ptr->getModuleName(), 0.0); + processing_time_.emplace(manager_ptr->name(), 0.0); } /** @@ -301,8 +301,9 @@ class PlannerManager */ void deleteExpiredModules(SceneModulePtr & module_ptr) const { - const auto manager = getManager(module_ptr); - manager->deleteModules(module_ptr); + module_ptr->onExit(); + module_ptr->publishRTCStatus(); + module_ptr.reset(); } /** @@ -348,41 +349,21 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief stop and remove not RUNNING modules in candidate_module_ptrs_. - */ - void clearNotRunningCandidateModules() - { - const auto it = std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - if (m->getCurrentStatus() != ModuleStatus::RUNNING) { - deleteExpiredModules(m); - return true; - } - return false; - }); - candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end()); - } - - /** - * @brief check if there is any RUNNING module in candidate_module_ptrs_. - */ - bool hasAnyRunningCandidateModule() - { - return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING; - }); - } - /** * @brief get current root lanelet. the lanelet is used for reference path generation. * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & data, bool success_lane_change = false) const { lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + if (success_lane_change) { + data->route_handler->getClosestPreferredLaneletWithinRoute( + data->self_odometry->pose.pose, &ret); + } else { + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + } RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } @@ -396,7 +377,7 @@ class PlannerManager { const auto itr = std::find_if( manager_ptrs_.begin(), manager_ptrs_.end(), - [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); }); + [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); }); if (itr == manager_ptrs_.end()) { throw std::domain_error("unknown manager name."); 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 4aa7f9ef142c6..9d89138f7cdfe 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 @@ -35,9 +35,9 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_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/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index 4e4a54af14b53..10a0d055a5a77 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 @@ -34,9 +34,9 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface DynamicAvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_); } 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 b6960aba10846..592a578f7fc72 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 @@ -33,9 +33,9 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface GoalPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_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/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index af65245ce4e3f..cf618b755b094 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -37,7 +37,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, const Direction direction, const LaneChangeModuleType type); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; @@ -55,7 +55,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager AvoidanceByLaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override; + std::unique_ptr createNewSceneModuleInstance() override; // void updateModuleParams(const std::vector & parameters) override; 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 0c6ec17e94a6a..ee5d539b3415d 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 @@ -38,6 +38,7 @@ using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::toHexString; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; +using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { @@ -73,56 +74,50 @@ class SceneModuleManagerInterface virtual ~SceneModuleManagerInterface() = default; - SceneModulePtr getNewModule() + void updateIdleModuleInstance() { - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onEntry(); - return idling_module_ptr_; + if (idle_module_ptr_) { + idle_module_ptr_->onEntry(); + return; } - idling_module_ptr_ = createNewSceneModuleInstance(); - return idling_module_ptr_; + idle_module_ptr_ = createNewSceneModuleInstance(); } - bool isExecutionRequested( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) const + bool isExecutionRequested(const BehaviorModuleOutput & previous_module_output) const { - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->updateData(); + idle_module_ptr_->setData(planner_data_); + idle_module_ptr_->setPreviousModuleOutput(previous_module_output); + idle_module_ptr_->updateData(); - return module_ptr->isExecutionRequested(); + return idle_module_ptr_->isExecutionRequested(); } void registerNewModule( - const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) + const SceneModuleObserver & observer, const BehaviorModuleOutput & previous_module_output) { - module_ptr->setIsSimultaneousExecutableAsApprovedModule( + if (observer.expired()) { + return; + } + + observer.lock()->setIsSimultaneousExecutableAsApprovedModule( enable_simultaneous_execution_as_approved_module_); - module_ptr->setIsSimultaneousExecutableAsCandidateModule( + observer.lock()->setIsSimultaneousExecutableAsCandidateModule( enable_simultaneous_execution_as_candidate_module_); - module_ptr->setData(planner_data_); - module_ptr->setPreviousModuleOutput(previous_module_output); - module_ptr->onEntry(); + observer.lock()->setData(planner_data_); + observer.lock()->setPreviousModuleOutput(previous_module_output); + observer.lock()->onEntry(); - registered_modules_.push_back(module_ptr); + observers_.push_back(observer); } - void deleteModules(SceneModulePtr & module_ptr) + void updateObserver() { - module_ptr->onExit(); - module_ptr->publishRTCStatus(); - - const auto itr = std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr); - - if (itr != registered_modules_.end()) { - registered_modules_.erase(itr); - } + const auto itr = std::remove_if( + observers_.begin(), observers_.end(), + [](const auto & observer) { return observer.expired(); }); - module_ptr.reset(); - idling_module_ptr_.reset(); - - pub_debug_marker_->publish(MarkerArray{}); + observers_.erase(itr, observers_.end()); } void publishVirtualWall() const @@ -134,32 +129,36 @@ class SceneModuleManagerInterface const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - const auto opt_stop_pose = m->getStopPose(); + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + const auto opt_stop_pose = m.lock()->getStopPose(); if (!!opt_stop_pose) { const auto virtual_wall = createStopVirtualWallMarker( - opt_stop_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_stop_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_slow_pose = m->getSlowPose(); + const auto opt_slow_pose = m.lock()->getSlowPose(); if (!!opt_slow_pose) { const auto virtual_wall = createSlowDownVirtualWallMarker( - opt_slow_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_slow_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto opt_dead_pose = m->getDeadPose(); + const auto opt_dead_pose = m.lock()->getDeadPose(); if (!!opt_dead_pose) { const auto virtual_wall = createDeadLineVirtualWallMarker( - opt_dead_pose.get(), m->name(), rclcpp::Clock().now(), marker_id); + opt_dead_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } - const auto module_specific_wall = m->getModuleVirtualWall(); + const auto module_specific_wall = m.lock()->getModuleVirtualWall(); appendMarkerArray(module_specific_wall, &markers); - m->resetWallPoses(); + m.lock()->resetWallPoses(); } pub_virtual_wall_->publish(markers); @@ -176,18 +175,22 @@ class SceneModuleManagerInterface const auto marker_offset = std::numeric_limits::max(); uint32_t marker_id = marker_offset; - for (const auto & m : registered_modules_) { - for (auto & marker : m->getInfoMarkers().markers) { + for (const auto & m : observers_) { + if (m.expired()) { + continue; + } + + for (auto & marker : m.lock()->getInfoMarkers().markers) { marker.id += marker_id; info_markers.markers.push_back(marker); } - for (auto & marker : m->getDebugMarkers().markers) { + for (auto & marker : m.lock()->getDebugMarkers().markers) { marker.id += marker_id; debug_markers.markers.push_back(marker); } - for (auto & marker : m->getDrivableLanesMarkers().markers) { + for (auto & marker : m.lock()->getDrivableLanesMarkers().markers) { marker.id += marker_id; drivable_lanes_markers.markers.push_back(marker); } @@ -195,10 +198,10 @@ class SceneModuleManagerInterface marker_id += marker_offset; } - if (registered_modules_.empty() && idling_module_ptr_ != nullptr) { - appendMarkerArray(idling_module_ptr_->getInfoMarkers(), &info_markers); - appendMarkerArray(idling_module_ptr_->getDebugMarkers(), &debug_markers); - appendMarkerArray(idling_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); + if (observers_.empty() && idle_module_ptr_ != nullptr) { + appendMarkerArray(idle_module_ptr_->getInfoMarkers(), &info_markers); + appendMarkerArray(idle_module_ptr_->getDebugMarkers(), &debug_markers); + appendMarkerArray(idle_module_ptr_->getDrivableLanesMarkers(), &drivable_lanes_markers); } pub_info_marker_->publish(info_markers); @@ -208,50 +211,62 @@ class SceneModuleManagerInterface bool exist(const SceneModulePtr & module_ptr) const { - return std::find(registered_modules_.begin(), registered_modules_.end(), module_ptr) != - registered_modules_.end(); + return std::any_of(observers_.begin(), observers_.end(), [&](const auto & observer) { + return !observer.expired() && observer.lock() == module_ptr; + }); } - bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } bool isSimultaneousExecutableAsApprovedModule() const { - if (registered_modules_.empty()) { + if (observers_.empty()) { return enable_simultaneous_execution_as_approved_module_; } - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsApprovedModule(); - }); + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_approved_module_; + } + return observer.lock()->isSimultaneousExecutableAsApprovedModule(); + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); } bool isSimultaneousExecutableAsCandidateModule() const { - if (registered_modules_.empty()) { + if (observers_.empty()) { return enable_simultaneous_execution_as_candidate_module_; } - return std::all_of( - registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { - return module->isSimultaneousExecutableAsCandidateModule(); - }); + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return enable_simultaneous_execution_as_candidate_module_; + } + return observer.lock()->isSimultaneousExecutableAsCandidateModule(); + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() { - std::for_each(registered_modules_.begin(), registered_modules_.end(), [](const auto & m) { - m->onExit(); - m->publishRTCStatus(); + std::for_each(observers_.begin(), observers_.end(), [](const auto & observer) { + if (!observer.expired()) { + observer.lock()->onExit(); + observer.lock()->publishRTCStatus(); + } }); - registered_modules_.clear(); - if (idling_module_ptr_ != nullptr) { - idling_module_ptr_->onExit(); - idling_module_ptr_->publishRTCStatus(); - idling_module_ptr_.reset(); + observers_.clear(); + + if (idle_module_ptr_ != nullptr) { + idle_module_ptr_->onExit(); + idle_module_ptr_->publishRTCStatus(); + idle_module_ptr_.reset(); } pub_debug_marker_->publish(MarkerArray{}); @@ -259,14 +274,16 @@ class SceneModuleManagerInterface size_t getPriority() const { return priority_; } - std::string getModuleName() const { return name_; } + std::string name() const { return name_; } + + std::vector getSceneModuleObservers() { return observers_; } - std::vector getSceneModules() { return registered_modules_; } + std::shared_ptr getIdleModule() { return std::move(idle_module_ptr_); } virtual void updateModuleParams(const std::vector & parameters) = 0; protected: - virtual std::shared_ptr createNewSceneModuleInstance() = 0; + virtual std::unique_ptr createNewSceneModuleInstance() = 0; rclcpp::Node * node_; @@ -286,9 +303,9 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; - std::vector registered_modules_; + std::vector observers_; - SceneModulePtr idling_module_ptr_; + std::unique_ptr idle_module_ptr_; std::unordered_map> rtc_interface_ptr_map_; 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 8a287e6920f33..b74be38c0faf9 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 @@ -34,9 +34,9 @@ class SideShiftModuleManager : public SceneModuleManagerInterface SideShiftModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_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/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index c86f9e415fdc7..9822e799b8ccf 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 @@ -33,9 +33,9 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface StartPlannerModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); - std::shared_ptr createNewSceneModuleInstance() override + std::unique_ptr createNewSceneModuleInstance() override { - return std::make_shared(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 29f1ee762abdf..6037daef13abe 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -131,7 +131,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod planner_manager_ = std::make_shared(*this, p.verbose); const auto register_and_create_publisher = [&](const auto & manager) { - const auto & module_name = manager->getModuleName(); + const auto & module_name = manager->name(); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( module_name, create_publisher(path_candidate_name_space + module_name, 1)); @@ -779,27 +779,31 @@ void BehaviorPathPlannerNode::publishPathCandidate( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_candidate_publishers_.count(manager->getModuleName()) == 0) { + if (path_candidate_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_candidate_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_candidate_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - const auto & status = module->getCurrentStatus(); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + const auto & status = observer.lock()->getCurrentStatus(); const auto candidate_path = std::invoke([&]() { if (status == ModuleStatus::SUCCESS || status == ModuleStatus::FAILURE) { // clear candidate path if the module is finished return convertToPath(nullptr, false, planner_data); } - return convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data); + return convertToPath( + observer.lock()->getPathCandidate(), observer.lock()->isExecutionReady(), planner_data); }); - path_candidate_publishers_.at(module->name())->publish(candidate_path); + path_candidate_publishers_.at(observer.lock()->name())->publish(candidate_path); } } } @@ -809,19 +813,22 @@ void BehaviorPathPlannerNode::publishPathReference( const std::shared_ptr & planner_data) { for (auto & manager : managers) { - if (path_reference_publishers_.count(manager->getModuleName()) == 0) { + if (path_reference_publishers_.count(manager->name()) == 0) { continue; } - if (manager->getSceneModules().empty()) { - path_reference_publishers_.at(manager->getModuleName()) + if (manager->getSceneModuleObservers().empty()) { + path_reference_publishers_.at(manager->name()) ->publish(convertToPath(nullptr, false, planner_data)); continue; } - for (auto & module : manager->getSceneModules()) { - path_reference_publishers_.at(module->name()) - ->publish(convertToPath(module->getPathReference(), true, planner_data)); + for (auto & observer : manager->getSceneModuleObservers()) { + if (observer.expired()) { + continue; + } + path_reference_publishers_.at(observer.lock()->name()) + ->publish(convertToPath(observer.lock()->getPathReference(), true, planner_data)); } } } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a3309968dd62c..4b5d36c56f268 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -209,14 +209,14 @@ std::vector PlannerManager::getRequestModules( }; for (const auto & manager_ptr : manager_ptrs_) { - stop_watch_.tic(manager_ptr->getModuleName()); + stop_watch_.tic(manager_ptr->name()); /** * don't launch candidate module if approved modules already exist. */ if (!approved_module_ptrs_.empty()) { if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -225,21 +225,20 @@ std::vector PlannerManager::getRequestModules( * launch new candidate module. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_same_name_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_same_name_module); if (itr == candidate_module_ptrs_.end()) { if (manager_ptr->canLaunchNewModule()) { - const auto new_module_ptr = manager_ptr->getNewModule(); - - if (manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - request_modules.emplace_back(new_module_ptr); + manager_ptr->updateIdleModuleInstance(); + if (manager_ptr->isExecutionRequested(previous_module_output)) { + request_modules.emplace_back(manager_ptr->getIdleModule()); } } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -249,7 +248,7 @@ std::vector PlannerManager::getRequestModules( * candidate. if locked, break this loop. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_block_module = [&name](const auto & m) { return m->name() == name && m->isLockedNewModuleLaunch(); }; @@ -259,7 +258,7 @@ std::vector PlannerManager::getRequestModules( if (itr != candidate_module_ptrs_.end()) { request_modules.clear(); request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); break; } } @@ -268,14 +267,14 @@ std::vector PlannerManager::getRequestModules( * module already exist. keep using it as candidate. */ { - const auto name = manager_ptr->getModuleName(); + const auto name = manager_ptr->name(); const auto find_launched_module = [&name](const auto & m) { return m->name() == name; }; const auto itr = std::find_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), find_launched_module); if (itr != candidate_module_ptrs_.end()) { request_modules.emplace_back(*itr); - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } } @@ -285,20 +284,20 @@ std::vector PlannerManager::getRequestModules( */ { if (!manager_ptr->canLaunchNewModule()) { - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); continue; } - const auto new_module_ptr = manager_ptr->getNewModule(); - if (!manager_ptr->isExecutionRequested(new_module_ptr, previous_module_output)) { - toc(manager_ptr->getModuleName()); + manager_ptr->updateIdleModuleInstance(); + if (!manager_ptr->isExecutionRequested(previous_module_output)) { + toc(manager_ptr->name()); continue; } - request_modules.emplace_back(new_module_ptr); + request_modules.emplace_back(manager_ptr->getIdleModule()); } - toc(manager_ptr->getModuleName()); + toc(manager_ptr->name()); } return request_modules; @@ -366,7 +365,8 @@ std::pair PlannerManager::runRequestModule const auto & manager_ptr = getManager(module_ptr); if (!manager_ptr->exist(module_ptr)) { - manager_ptr->registerNewModule(module_ptr, previous_module_output); + manager_ptr->registerNewModule( + std::weak_ptr(module_ptr), previous_module_output); } results.emplace(module_ptr->name(), run(module_ptr, data, previous_module_output)); @@ -393,6 +393,9 @@ std::pair PlannerManager::runRequestModule executable_modules.erase( std::remove_if(executable_modules.begin(), executable_modules.end(), remove_expired_modules), executable_modules.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -439,8 +442,6 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); @@ -472,6 +473,8 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrupdateObserver(); }); debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); } @@ -492,6 +495,9 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrupdateObserver(); }); + if (itr != approved_module_ptrs_.end()) { clearCandidateModules(); } @@ -514,59 +520,31 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != ModuleStatus::SUCCESS; }); - - // convert reverse iterator -> iterator - const auto success_itr = std::prev(not_success_itr).base() - 1; - /** - * there is no succeeded module. return. + * remove success module immediately. if lane change module has succeeded, update root lanelet. */ - if (success_itr == approved_module_ptrs_.end()) { - return approved_modules_output; - } - - const auto lane_change_itr = std::find_if( - success_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - - /** - * remove success modules according to Last In First Out(LIFO) policy. when the next module is in - * ModuleStatus::RUNNING, the previous module keeps running even if it is in - * ModuleStatus::SUCCESS. - */ - if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); + { + const auto success_module_itr = std::partition( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + const auto success_lane_change = std::any_of( + success_module_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - return approved_modules_output; - } + if (success_lane_change) { + root_lanelet_ = updateRootLanelet(data, true); + } - /** - * as an exception, when there is lane change module is in succeeded modules, it doesn't remove - * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the - * root lanelet is updated at the moment of lane change module's unregistering, and that causes - * change First In module's input. - */ - if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { + std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); - root_lanelet_ = updateRootLanelet(data); - - return approved_modules_output; + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } return approved_modules_output; @@ -601,6 +579,9 @@ void PlannerManager::updateCandidateModules( std::remove_if( candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), candidate_to_remove), candidate_module_ptrs_.end()); + + std::for_each( + manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); }); } /** @@ -699,8 +680,8 @@ void PlannerManager::print() const string_stream << "-----------------------------------------------------------\n"; string_stream << "registered modules: "; for (const auto & m : manager_ptrs_) { - string_stream << "[" << m->getModuleName() << "]"; - max_string_num = std::max(max_string_num, m->getModuleName().length()); + string_stream << "[" << m->name() << "]"; + max_string_num = std::max(max_string_num, m->name().length()); } string_stream << "\n"; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index ec2ede8373831..13b3344dc7c39 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -366,8 +366,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorsharp_shift_filter_threshold); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 27b467b3c9a74..8cf80f3289e9c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -171,8 +171,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( p->end_duration_to_avoid_oncoming_object); } - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index caa75927f4062..bfa1487f05411 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -246,8 +246,8 @@ void GoalPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } 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 18c669a207095..7c12579bf34f5 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 @@ -133,14 +133,14 @@ LaneChangeModuleManager::LaneChangeModuleManager( parameters_ = std::make_shared(p); } -std::shared_ptr LaneChangeModuleManager::createNewSceneModuleInstance() +std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() { if (type_ == LaneChangeModuleType::NORMAL) { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, std::make_unique(parameters_, direction_)); } @@ -155,8 +155,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } @@ -271,10 +271,10 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( avoidance_parameters_ = std::make_shared(p); } -std::shared_ptr +std::unique_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { - return std::make_shared( + return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); } diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5c92d7ced6036..5a039a98f2e52 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -51,8 +51,8 @@ void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = "side_shift."; // updateParam(parameters, ns + ..., ...); - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 881da2b52720a..3b6801a7b0309 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -99,13 +99,17 @@ void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] std::string ns = name_ + "."; - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&](const auto & m) { - const auto start_planner_ptr = std::dynamic_pointer_cast(m); - start_planner_ptr->updateModuleParams(p); - start_planner_ptr->setInitialIsSimultaneousExecutableAsApprovedModule( - enable_simultaneous_execution_as_approved_module_); - start_planner_ptr->setInitialIsSimultaneousExecutableAsCandidateModule( - enable_simultaneous_execution_as_candidate_module_); + std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { + if (!observer.expired()) { + const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); + if (start_planner_ptr) { + start_planner_ptr->updateModuleParams(p); + start_planner_ptr->setInitialIsSimultaneousExecutableAsApprovedModule( + enable_simultaneous_execution_as_approved_module_); + start_planner_ptr->setInitialIsSimultaneousExecutableAsCandidateModule( + enable_simultaneous_execution_as_candidate_module_); + } + } }); } } // namespace behavior_path_planner diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 3c3ddbc1acaf2..b298d69949610 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -301,6 +301,8 @@ class RouteHandler bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 358c44a295a29..33d44851e73f1 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -788,6 +788,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const