Merge branch 'beta/v0.41' into cherry-pick/10061

common/autoware_component_interface_tools/package.xml

@@ -10,10 +10,15 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>component_interface_utils</depend>
+  <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
   <depend>tier4_system_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -44,8 +44,16 @@ def __init__(self, context):
             "/perception/obstacle_segmentation/single_frame/pointcloud"
         )
         self.output_topic = "/perception/obstacle_segmentation/pointcloud"
-        self.use_single_frame_filter = self.ground_segmentation_param["use_single_frame_filter"]
-        self.use_time_series_filter = self.ground_segmentation_param["use_time_series_filter"]
+        self.use_single_frame_filter = LaunchConfiguration("use_single_frame_filter").perform(
+            context
+        )
+        self.use_time_series_filter = LaunchConfiguration("use_time_series_filter").perform(context)
+        # check if self.use_single_frame_filter is bool
+        if isinstance(self.use_single_frame_filter, str):
+            self.use_single_frame_filter = self.use_single_frame_filter.lower() == "true"
+        # check if self.use_time_series_filter is bool
+        if isinstance(self.use_time_series_filter, str):
+            self.use_time_series_filter = self.use_time_series_filter.lower() == "true"
 
     def get_vehicle_info(self):
         # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -75,6 +75,9 @@
   <arg name="camera_info7" default="/sensing/camera/camera7/camera_info"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="6" description="choose image raw number(1-8)"/>
+  <arg name="pointcloud_container_name" default="pointcloud_container"/>
+
+  <!-- Pipeline junctions -->
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_low_height_cropbox" default="true" description="use low height crop filter in the euclidean clustering"/>
@@ -88,10 +91,11 @@
     default="false"
     description="if use_empty_dynamic_object_publisher:=true, /perception/object_recognition/objects topic has an empty DynamicObjectArray"
   />
-  <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="use_object_validator" default="true" description="use obstacle_pointcloud based object validator"/>
   <arg name="objects_validation_method" default="obstacle_pointcloud" description="options: `obstacle_pointcloud` or `occupancy_grid`"/>
   <arg name="use_perception_online_evaluator" default="false" description="use perception online evaluator"/>
+  <arg name="use_obstacle_segmentation_single_frame_filter" description="use single frame filter at the ground segmentation"/>
+  <arg name="use_obstacle_segmentation_time_series_filter" description="use time series filter at the ground segmentation"/>
 
   <!-- Traffic light recognition parameters -->
   <arg name="use_traffic_light_recognition" default="false"/>
@@ -171,14 +175,20 @@
         <arg name="use_multithread" value="true"/>
         <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
         <arg name="input/pointcloud" value="$(var perception_pointcloud)"/>
+        <arg name="obstacle_segmentation_ground_segmentation_param_path" value="$(var obstacle_segmentation_ground_segmentation_param_path)"/>
+        <arg name="use_single_frame_filter" value="$(var use_obstacle_segmentation_single_frame_filter)"/>
+        <arg name="use_time_series_filter" value="$(var use_obstacle_segmentation_time_series_filter)"/>
       </include>
     </group>
 
     <!-- Occupancy grid map module -->
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
+      <let name="unfiltered_obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
+      <let name="unfiltered_obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud" if="$(var use_obstacle_segmentation_single_frame_filter)"/>
+      <let name="unfiltered_obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud" if="$(var use_obstacle_segmentation_time_series_filter)"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
-        <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud"/>
+        <arg name="input/obstacle_pointcloud" value="$(var unfiltered_obstacle_pointcloud)"/>
         <arg name="input/raw_pointcloud" value="$(var perception_pointcloud)"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>

launch/tier4_system_launch/launch/system.launch.xml

@@ -18,6 +18,7 @@
 
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
+  <arg name="launch_system_recover_operator" default="false" description="launch recover operator"/>
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
@@ -122,4 +123,9 @@
       <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
     </include>
   </group>
+
+  <!-- System Recover Operator -->
+  <group if="$(var launch_system_recover_operator)">
+    <node pkg="autoware_diagnostic_graph_utils" exec="recovery_node" name="recovery"/>
+  </group>
 </launch>

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -234,8 +234,7 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_
   sensor_msgs::PointCloud2Iterator<float> iter_painted_z(painted_pointcloud_msg, "z");
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(tmp, "x"), iter_y(tmp, "y"),
        iter_z(tmp, "z");
-       iter_x != iter_x.end();
-       ++iter_x, ++iter_y, ++iter_z, ++iter_painted_x, ++iter_painted_y, ++iter_painted_z) {
+       iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
     if (
       *iter_x <= pointcloud_range.at(0) || *iter_x >= pointcloud_range.at(3) ||
       *iter_y <= pointcloud_range.at(1) || *iter_y >= pointcloud_range.at(4)) {
@@ -245,6 +244,9 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_
       *iter_painted_y = *iter_y;
       *iter_painted_z = *iter_z;
       j += painted_point_step;
+      ++iter_painted_x;
+      ++iter_painted_y;
+      ++iter_painted_z;
     }
   }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -384,7 +384,6 @@ In addition, the safety check has a time hysteresis, and if the path is judged "
 
 | Name                                 | Unit  | Type   | Description                                                                                              | Default value                |
 | :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
-| enable_safety_check                  | [-]   | bool   | flag whether to use safety check                                                                         | true                         |
 | method                               | [-]   | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
 | keep_unsafe_time                     | [s]   | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
 | check_all_predicted_path             | -     | bool   | Flag to check all predicted paths                                                                        | true                         |

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp

@@ -15,7 +15,7 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
 
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp"
 #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
 
@@ -49,15 +49,13 @@ class PathDecisionStateController
    * @brief update current state and save old current state to prev state
    */
   void transit_state(
-    const bool found_pull_over_path, const rclcpp::Time & now,
+    const std::optional<PullOverPath> & pull_over_path_opt, const rclcpp::Time & now,
     const autoware_perception_msgs::msg::PredictedObjects & static_target_objects,
     const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects,
-    const std::optional<GoalCandidate> modified_goal_opt,
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
-    const std::optional<PullOverPath> & pull_over_path,
+    const GoalSearcher & goal_searcher,
     std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded);
 
   PathDecisionState get_current_state() const { return current_state_; }
@@ -71,14 +69,12 @@ class PathDecisionStateController
    * @brief update current state and save old current state to prev state
    */
   PathDecisionState get_next_state(
-    const bool found_pull_over_path, const rclcpp::Time & now,
+    const std::optional<PullOverPath> & pull_over_path_opt, const rclcpp::Time & now,
     const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
-    const std::optional<GoalCandidate> modified_goal_opt,
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
-    const std::optional<PullOverPath> & pull_over_path_opt,
+    const GoalSearcher & goal_searcher,
     std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded) const;
 };
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp

@@ -0,0 +1,36 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_
+#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <vector>
+
+namespace autoware::behavior_path_planner
+{
+struct GoalCandidate
+{
+  geometry_msgs::msg::Pose goal_pose{};
+  double distance_from_original_goal{0.0};
+  double lateral_offset{0.0};
+  size_t id{0};
+  bool is_safe{true};
+  size_t num_objects_to_avoid{0};
+};
+using GoalCandidates = std::vector<GoalCandidate>;
+
+}  // namespace autoware::behavior_path_planner
+#endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -73,14 +73,6 @@ struct GoalPlannerDebugData
   utils::path_safety_checker::CollisionCheckDebugMap collision_check{};
 };
 
-struct LastApprovalData
-{
-  LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {}
-
-  rclcpp::Time time{};
-  Pose pose{};
-};
-
 struct PullOverContextData
 {
   PullOverContextData() = delete;
@@ -335,7 +327,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
-  std::shared_ptr<GoalSearcherBase> goal_searcher_;
+  std::optional<GoalSearcher> goal_searcher_{};
   GoalCandidates goal_candidates_{};
 
   bool use_bus_stop_area_{false};
@@ -352,7 +344,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverContextData> context_data_{std::nullopt};
   // path_decision_controller is updated in updateData(), and used in plan()
   PathDecisionStateController path_decision_controller_{getLogger()};
-  std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
+  std::optional<rclcpp::Time> decided_time_{};
 
   // approximate distance from the start point to the end point of pull_over.
   // this is used as an assumed value to decelerate, etc., before generating the actual path.
@@ -364,7 +356,8 @@ class GoalPlannerModule : public SceneModuleInterface
   mutable GoalPlannerDebugData debug_data_;
 
   // goal seach
-  GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const;
+  GoalCandidates generateGoalCandidates(
+    GoalSearcher & goal_searcher, const bool use_bus_stop_area) const;
 
   /*
    * state transitions and plan function used in each state
@@ -433,7 +426,6 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
-    const GoalCandidates & goal_candidates,
     const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const;
 
   // lanes and drivable area

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -15,7 +15,12 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_
 
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_planner_common/data_manager.hpp"
+#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <memory>
 #include <vector>
@@ -24,32 +29,45 @@ namespace autoware::behavior_path_planner
 {
 using autoware::universe_utils::LinearRing2d;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
+using autoware::universe_utils::MultiPolygon2d;
 
-class GoalSearcher : public GoalSearcherBase
+class GoalSearcher
 {
 public:
-  GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
+  static GoalSearcher create(
+    const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
+    const std::shared_ptr<const PlannerData> & planner_data);
 
+public:
   GoalCandidates search(
-    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) override;
+    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area);
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data,
-    const PredictedObjects & objects) const override;
+    const PredictedObjects & objects) const;
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
-  GoalCandidate getClosetGoalCandidateAlongLanes(
+  std::optional<GoalCandidate> getClosestGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data) const;
   bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data,
-    const PredictedObjects & objects) const override;
+    const PredictedObjects & objects) const;
+
+  MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
 
 private:
+  GoalSearcher(
+    const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
+    const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes,
+    const lanelet::BasicPolygons2d & no_parking_area_polygons,
+    const lanelet::BasicPolygons2d & no_stopping_area_polygons,
+    const lanelet::BasicPolygons2d & bus_stop_area_polygons);
+
   void countObjectsToAvoid(
     GoalCandidates & goal_candidates, const PredictedObjects & objects,
     const std::shared_ptr<const PlannerData> & planner_data,
@@ -64,11 +82,16 @@ class GoalSearcher : public GoalSearcherBase
     const Pose & ego_pose, const PredictedObjects & objects,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data) const;
-  BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
-  BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
 
-  LinearRing2d vehicle_footprint_{};
-  bool left_side_parking_{true};
+  const GoalPlannerParameters parameters_;
+  const LinearRing2d vehicle_footprint_;
+  const bool left_side_parking_;
+  const lanelet::ConstLanelets pull_over_lanes_;
+  const lanelet::BasicPolygons2d no_parking_area_polygons_;
+  const lanelet::BasicPolygons2d no_stopping_area_polygons_;
+  const lanelet::BasicPolygons2d bus_stop_area_polygons_;
+
+  MultiPolygon2d area_polygons_{};
 };
 }  // namespace autoware::behavior_path_planner