Merge branch 'main' into feat/modernize-rviz-ui

localization/localization_error_monitor/src/diagnostics.cpp

@@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy(
   diagnostic_msgs::msg::DiagnosticStatus stat;
 
   diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "localization_accuracy";
+  key_value.key = "localization_error_ellipse";
   key_value.value = std::to_string(ellipse_size);
   stat.values.push_back(key_value);
 
@@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection
   diagnostic_msgs::msg::DiagnosticStatus stat;
 
   diagnostic_msgs::msg::KeyValue key_value;
-  key_value.key = "localization_accuracy_lateral_direction";
+  key_value.key = "localization_error_ellipse_lateral_direction";
   key_value.value = std::to_string(ellipse_size);
   stat.values.push_back(key_value);
 

localization/yabloc/yabloc_common/README.md

@@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2.
 
 ### Parameters
 
-| Name              | Type | Description                                              |
-| ----------------- | ---- | -------------------------------------------------------- |
-| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. |
-| `K`               | int  | parameter for nearest k search                           |
-| `R`               | int  | parameter for radius search                              |
+{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }}
 
 ## ll2_decomposer
 

localization/yabloc/yabloc_image_processing/README.md

@@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs
 
 ### Parameters
 
-| Name                              | Type   | Description                                                        |
-| --------------------------------- | ------ | ------------------------------------------------------------------ |
-| `target_height_ratio`             | double | height on the image to retrieve the candidate road surface         |
-| `target_candidate_box_width`      | int    | size of the square area to search for candidate road surfaces      |
-| `pickup_additional_graph_segment` | bool   | if this is true, additional regions of similar color are retrieved |
-| `similarity_score_threshold`      | double | threshold for picking up additional areas                          |
-| `sigma`                           | double | parameters for cv::ximgproc::segmentation                          |
-| `k`                               | double | parameters for cv::ximgproc::segmentation                          |
-| `min_size`                        | double | parameters for cv::ximgproc::segmentation                          |
+{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }}
 
 ## segment_filter
 
@@ -89,7 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r
 
 ### Parameters
 
-{{ json_to_markdown("localization/yabloc/yabloc_common/schema/segment_filter.schema.json") }}
+{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }}
 
 ## undistort
 
@@ -120,7 +112,7 @@ This is to avoid redundant decompression within Autoware.
 
 ### Parameters
 
-{{ json_to_markdown("localization/yabloc/yabloc_common/schema/undistort.schema.json") }}
+{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }}
 
 #### about tf_static overriding
 

perception/cluster_merger/CMakeLists.txt

@@ -24,4 +24,5 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
+  config
 )

perception/cluster_merger/config/cluster_merger.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    output_frame_id: "base_link"

perception/cluster_merger/launch/cluster_merger.launch.xml

@@ -3,12 +3,12 @@
   <arg name="output/clusters" default="output/clusters"/>
   <arg name="input/cluster0" default="input/cluster0"/>
   <arg name="input/cluster1" default="input/cluster1"/>
-  <arg name="output_frame_id" default="base_link"/>
+  <arg name="param_path" default="$(find-pkg-share cluster_merger)/config/cluster_merger.param.yaml"/>
   <!-- Node -->
   <node pkg="cluster_merger" exec="cluster_merger_node" name="cluster_merger" output="screen">
     <remap from="~/output/clusters" to="$(var output/clusters)"/>
     <remap from="input/cluster0" to="$(var input/cluster0)"/>
     <remap from="input/cluster1" to="$(var input/cluster1)"/>
-    <param name="output_frame_id" value="$(var output_frame_id)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>

planning/behavior_path_dynamic_avoidance_module/src/scene.cpp

@@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
     current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   output.reference_path = getPreviousModuleOutput().reference_path;
   output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+  output.modified_goal = getPreviousModuleOutput().modified_goal;
 
   return output;
 }

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -55,7 +55,7 @@ class NormalLaneChange : public LaneChangeBase
 
   BehaviorModuleOutput generateOutput() override;
 
-  void extendOutputDrivableArea(BehaviorModuleOutput & output) override;
+  void extendOutputDrivableArea(BehaviorModuleOutput & output) const override;
 
   void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
 
@@ -65,7 +65,7 @@ class NormalLaneChange : public LaneChangeBase
 
   void resetParameters() override;
 
-  TurnSignalInfo updateOutputTurnSignal() override;
+  TurnSignalInfo updateOutputTurnSignal() const override;
 
   bool calcAbortPath() override;
 
@@ -141,7 +141,7 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
     const bool check_safety = true) const override;
 
-  TurnSignalInfo calcTurnSignalInfo() override;
+  TurnSignalInfo calcTurnSignalInfo() const override;
 
   bool isValidPath(const PathWithLaneId & path) const override;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -66,15 +66,15 @@ class LaneChangeBase
 
   virtual BehaviorModuleOutput generateOutput() = 0;
 
-  virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0;
+  virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0;
 
   virtual PathWithLaneId getReferencePath() const = 0;
 
   virtual std::optional<PathWithLaneId> extendPath() = 0;
 
   virtual void resetParameters() = 0;
 
-  virtual TurnSignalInfo updateOutputTurnSignal() = 0;
+  virtual TurnSignalInfo updateOutputTurnSignal() const = 0;
 
   virtual bool hasFinishedLaneChange() const = 0;
 
@@ -225,7 +225,7 @@ class LaneChangeBase
     LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
     const bool is_stuck, const bool check_safety) const = 0;
 
-  virtual TurnSignalInfo calcTurnSignalInfo() = 0;
+  virtual TurnSignalInfo calcTurnSignalInfo() const = 0;
 
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -183,7 +183,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
   return output;
 }
 
-void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
+void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const
 {
   const auto & dp = planner_data_->drivable_area_expansion_parameters;
 
@@ -431,7 +431,7 @@ void NormalLaneChange::resetParameters()
   RCLCPP_DEBUG(logger_, "reset all flags and debug information.");
 }
 
-TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()
+TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
 {
   TurnSignalInfo turn_signal_info = calcTurnSignalInfo();
   const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal(
@@ -1447,7 +1447,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   return safety_status;
 }
 
-TurnSignalInfo NormalLaneChange::calcTurnSignalInfo()
+TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
 {
   const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {
     double accumulated_length = 0.0;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
     return output;
   }
 
-  const auto move_to_end = [](auto & modules, const auto & cond) {
-    auto itr = modules.begin();
-    while (itr != modules.end()) {
-      const auto satisfied_exit_cond =
-        std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
-
-      if (satisfied_exit_cond) {
-        return;
-      }
+  // move modules whose keep last flag is true to end of the approved_module_ptrs_.
+  // if there are multiple keep last modules, sort by priority
+  {
+    const auto move_to_end = [](auto & modules, const auto & module_to_move) {
+      auto itr = std::find(modules.begin(), modules.end(), module_to_move);
 
-      if (cond(*itr)) {
+      if (itr != modules.end()) {
         auto tmp = std::move(*itr);
-        itr = modules.erase(itr);
-        modules.insert(modules.end(), std::move(tmp));
-      } else {
-        itr++;
+        modules.erase(itr);
+        modules.push_back(std::move(tmp));
       }
-    }
-  };
+    };
 
-  // move modules whose keep last flag is true to end of the approved_module_ptrs_.
-  {
-    const auto keep_last_module_cond = [this](const auto & m) {
-      return getManager(m)->isKeepLast();
+    const auto get_sorted_keep_last_modules = [this](const auto & modules) {
+      std::vector<SceneModulePtr> keep_last_modules;
+
+      std::copy_if(
+        modules.begin(), modules.end(), std::back_inserter(keep_last_modules),
+        [this](const auto & m) { return getManager(m)->isKeepLast(); });
+
+      // sort by priority (low -> high)
+      std::sort(
+        keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) {
+          return getManager(a)->getPriority() < getManager(b)->getPriority();
+        });
+
+      return keep_last_modules;
     };
-    move_to_end(approved_module_ptrs_, keep_last_module_cond);
+
+    for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) {
+      move_to_end(approved_module_ptrs_, module);
+    }
   }
 
   // lock approved modules besides last one
@@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * remove success module immediately. if lane change module has succeeded, update root lanelet.
    */
   {
+    const auto move_to_end = [](auto & modules, const auto & cond) {
+      auto itr = modules.begin();
+      while (itr != modules.end()) {
+        const auto satisfied_exit_cond =
+          std::all_of(itr, modules.end(), [&cond](const auto & m) { return cond(m); });
+
+        if (satisfied_exit_cond) {
+          return;
+        }
+
+        if (cond(*itr)) {
+          auto tmp = std::move(*itr);
+          itr = modules.erase(itr);
+          modules.insert(modules.end(), std::move(tmp));
+        } else {
+          itr++;
+        }
+      }
+    };
     const auto success_module_cond = [](const auto & m) {
       return m->getCurrentStatus() == ModuleStatus::SUCCESS;
     };

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -1884,6 +1884,10 @@ DrivableAreaInfo combineDrivableAreaInfo(
     drivable_area_info1.enable_expanding_intersection_areas ||
     drivable_area_info2.enable_expanding_intersection_areas;
 
+  // drivable margin
+  combined_drivable_area_info.drivable_margin =
+    std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin);
+
   return combined_drivable_area_info;
 }
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override;
+  bool canTransitIdleToRunningState() override { return true; }
 
   /**
    * @brief init member variables.

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -347,11 +347,6 @@ bool StartPlannerModule::canTransitSuccessState()
   return hasFinishedPullOut();
 }
 
-bool StartPlannerModule::canTransitIdleToRunningState()
-{
-  return isActivated() && !isWaitingApproval();
-}
-
 BehaviorModuleOutput StartPlannerModule::plan()
 {
   if (isWaitingApproval()) {
@@ -1317,6 +1312,7 @@ bool StartPlannerModule::planFreespacePath()
 void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
   if (status_.planner_type == PlannerType::FREESPACE) {
+    std::cerr << "Freespace planner updated drivable area." << std::endl;
     const double drivable_area_margin = planner_data_->parameters.vehicle_width;
     output.drivable_area_info.drivable_margin =
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;

planning/behavior_velocity_crosswalk_module/README.md

@@ -196,7 +196,7 @@ In the `stuck_vehicle` namespace, the following parameters are defined.
 | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- |
 | `stuck_vehicle_velocity`           | [m/s]   | double | maximum velocity threshold whether the target vehicle is stopped or not |
 | `max_stuck_vehicle_lateral_offset` | [m]     | double | maximum lateral offset of the target vehicle position                   |
-| `stuck_vehicle_attention_range`    | [m]     | double | detection area length ahead of the crosswalk                            |
+| `required_clearance`               | [m]     | double | clearance to be secured between the ego and the ahead vehicle           |
 | `min_acc`                          | [m/ss]  | double | minimum acceleration to stop                                            |
 | `min_jerk`                         | [m/sss] | double | minimum jerk to stop                                                    |
 | `max_jerk`                         | [m/sss] | double | maximum jerk to stop                                                    |

planning/motion_velocity_smoother/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
   <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <maintainer email="[email protected]">Satoshi Ota</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Takamasa Horibe</author>

simulator/fault_injection/config/fault_injection.param.yaml

@@ -4,7 +4,7 @@
       vehicle_is_out_of_lane: "lane_departure"
       trajectory_deviation_is_high: "trajectory_deviation"
       localization_matching_score_is_low: "ndt_scan_matcher"
-      localization_accuracy_is_low: "localization_accuracy"
+      localization_accuracy_is_low: "localization_error_ellipse"
       map_version_is_different: "map_version"
       trajectory_is_invalid: "trajectory_point_validation"
       cpu_temperature_is_high: "CPU Temperature"

system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml

@@ -24,9 +24,9 @@
               contains: ["ndt_scan_matcher"]
               timeout: 1.0
 
-            localization_accuracy:
+            localization_error_ellipse:
               type: diagnostic_aggregator/GenericAnalyzer
-              path: localization_accuracy
+              path: localization_error_ellipse
               contains: ["localization: localization_error_monitor"]
               timeout: 1.0
 

system/system_error_monitor/config/system_error_monitor.param.yaml

@@ -24,7 +24,7 @@
 
         /autoware/localization/node_alive_monitoring: default
         /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" }
-        /autoware/localization/performance_monitoring/localization_accuracy: default
+        /autoware/localization/performance_monitoring/localization_error_ellipse: default
 
         /autoware/map/node_alive_monitoring: default
 

system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml

@@ -24,7 +24,7 @@
 
         /autoware/localization/node_alive_monitoring: default
         # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" }
-        # /autoware/localization/performance_monitoring/localization_accuracy: default
+        # /autoware/localization/performance_monitoring/localization_error_ellipse: default
 
         /autoware/map/node_alive_monitoring: default