[pull] main from autowarefoundation:main

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<double>` | 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<double>` | Generalized IoU threshold for each class                                                                                                                                                                                              |
 
 ## Tips
 

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]

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<int, double> generalized_iou_threshold;
     std::map<int /*class label*/, double /*distance_threshold*/> distance_threshold_map;
   } overlapped_judge_param_;
 };

perception/object_merger/launch/object_association_merger.launch.xml

@@ -14,7 +14,7 @@
     <param from="$(var data_association_matrix_path)"/>
     <param from="$(var distance_threshold_list_path)"/>
     <param name="priority_mode" value="$(var priority_mode)"/>
-    <param name="generalized_iou_threshold" value="-0.6"/>
     <param name="precision_threshold_to_judge_overlapped" value="0.4"/>
+    <param name="remove_overlapped_unknown_objects" value="true"/>
   </node>
 </launch>

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<int, double> distance_threshold_map, const double generalized_iou_threshold)
+  std::map<int, double> distance_threshold_map,
+  const std::map<int, double> 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<double>("recall_threshold_to_judge_overlapped", 0.5);
   overlapped_judge_param_.generalized_iou_threshold =
-    declare_parameter<double>("generalized_iou_threshold");
+    convertListToClassMap(declare_parameter<std::vector<double>>("generalized_iou_threshold"));
 
   // get distance_threshold_map from distance_threshold_list
   /** TODO(Shin-kyoto):

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<PlannerData> & data) const
+  lanelet::ConstLanelet updateRootLanelet(
+    const std::shared_ptr<PlannerData> & 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.");

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<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

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<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<DynamicAvoidanceModule>(
+    return std::make_unique<DynamicAvoidanceModule>(
       name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 

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<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<GoalPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<GoalPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

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<SceneModuleInterface> createNewSceneModuleInstance() override;
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
@@ -55,7 +55,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
   AvoidanceByLaneChangeModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
 
   // void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;