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

.github/CODEOWNERS

@@ -175,7 +175,7 @@ planning/behavior_velocity_occlusion_spot_module/** [email protected]
 planning/behavior_velocity_out_of_lane_module/** [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner_common/** [email protected] [email protected] [email protected] [email protected] [email protected]
-planning/behavior_velocity_run_out_module/** [email protected] [email protected] [email protected] [email protected] [email protected]
+planning/behavior_velocity_run_out_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_speed_bump_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_stop_line_module/** [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_template_module/** [email protected]

launch/tier4_vehicle_launch/launch/vehicle.launch.xml

@@ -6,6 +6,7 @@
   <arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
   <arg name="initial_engage_state" default="false" description="/vehicle/engage state after starting Autoware"/>
   <arg name="config_dir" default="$(find-pkg-share $(var sensor_model)_description)/config" description="path to dir where sensors_calibration.yaml, etc. are located"/>
+  <arg name="raw_vehicle_cmd_converter_param_path" default="$(find-pkg-share raw_vehicle_cmd_converter)/config/raw_vehicle_cmd_converter.param.yaml"/>
 
   <let name="vehicle_launch_pkg" value="$(find-pkg-share $(var vehicle_model)_launch)"/>
 
@@ -21,6 +22,7 @@
   <group if="$(var launch_vehicle_interface)">
     <include file="$(var vehicle_launch_pkg)/launch/vehicle_interface.launch.xml">
       <arg name="vehicle_id" value="$(var vehicle_id)"/>
+      <arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
       <arg name="initial_engage_state" value="$(var initial_engage_state)"/>
     </include>
   </group>

planning/behavior_path_dynamic_avoidance_module/src/scene.cpp

@@ -1221,7 +1221,6 @@ std::optional<MinMaxValue> DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi
       prev_object->ref_path_points_for_obj_poly,
       ref_path_points_for_obj_poly.at(obj_point_idx).point.pose.position));
 
-    std::cerr << paths_lat_diff << std::endl;
     constexpr double min_paths_lat_diff = 0.3;
     if (paths_lat_diff < min_paths_lat_diff) {
       return true;

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -230,6 +230,13 @@ struct LastApprovalData
   Pose pose{};
 };
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
 struct PreviousPullOverData
 {
   struct SafetyStatus
@@ -240,18 +247,14 @@ struct PreviousPullOverData
 
   void reset()
   {
-    stop_path = nullptr;
-    stop_path_after_approval = nullptr;
     found_path = false;
     safety_status = SafetyStatus{};
-    has_decided_path = false;
+    deciding_path_status = DecidingPathStatusWithStamp{};
   }
 
-  std::shared_ptr<PathWithLaneId> stop_path{nullptr};
-  std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
   bool found_path{false};
   SafetyStatus safety_status{};
-  bool has_decided_path{false};
+  DecidingPathStatusWithStamp deciding_path_status{};
 };
 
 // store stop_pose_ pointer with reason string
@@ -387,7 +390,7 @@ class GoalPlannerModule : public SceneModuleInterface
   ThreadSafeData thread_safe_data_;
 
   std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
-  PreviousPullOverData prev_data_{nullptr};
+  PreviousPullOverData prev_data_{};
 
   // 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.
@@ -428,7 +431,7 @@ class GoalPlannerModule : public SceneModuleInterface
   void decelerateBeforeSearchStart(
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath() const;
-  PathWithLaneId generateFeasibleStopPath() const;
+  PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;
 
   void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
@@ -443,6 +446,8 @@ class GoalPlannerModule : public SceneModuleInterface
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
+  bool hasNotDecidedPath() const;
+  DecidingPathStatusWithStamp checkDecidingPathStatus() const;
   void decideVelocity();
   bool foundPullOverPath() const;
   void updateStatus(const BehaviorModuleOutput & output);
@@ -477,19 +482,8 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // output setter
   void setOutput(BehaviorModuleOutput & output) const;
-  void setStopPath(BehaviorModuleOutput & output) const;
-  void updatePreviousData(const BehaviorModuleOutput & output);
+  void updatePreviousData();
 
-  /**
-   * @brief Sets a stop path in the current path based on safety conditions and previous paths.
-   *
-   * This function sets a stop path in the current path. Depending on whether the previous safety
-   * judgement against dynamic objects were safe or if a previous stop path existed, it either
-   * generates a new stop path or uses the previous stop path.
-   *
-   * @param output BehaviorModuleOutput
-   */
-  void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
   void setModifiedGoal(BehaviorModuleOutput & output) const;
   void setTurnSignalInfo(BehaviorModuleOutput & output) const;
 

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase
 
   GoalCandidates search() override;
   void update(GoalCandidates & goal_candidates) const override;
+
+  // 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(
     const GoalCandidates & goal_candidates) const override;
+  bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp

@@ -60,6 +60,8 @@ class GoalSearcherBase
   virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; }
   virtual GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates) const = 0;
+  virtual bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

@@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance);
+lanelet::ConstLanelets generateExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset);
+PredictedObjects extractObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
+PredictedObjects extractStaticObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects,
+  const double velocity_thresh);
 
 double calcLateralDeviationBetweenPaths(
   const PathWithLaneId & reference_path, const PathWithLaneId & target_path);