Merge pull request autowarefoundation#486 from tier4/sync-upstream

chore: sync upstream

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -44,6 +44,7 @@
   <arg name="freespace_planner_param_path"/>
   <!-- motion -->
   <arg name="obstacle_avoidance_planner_param_path"/>
+  <arg name="path_sampler_param_path"/>
   <arg name="obstacle_velocity_limiter_param_path"/>
   <arg name="surround_obstacle_checker_param_path"/>
   <arg name="obstacle_stop_planner_param_path"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -64,6 +64,27 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # path sampler
+    with open(LaunchConfiguration("path_sampler_param_path").perform(context), "r") as f:
+        path_sampler_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    path_sampler_component = ComposableNode(
+        package="path_sampler",
+        plugin="path_sampler::PathSampler",
+        name="path_sampler",
+        namespace="",
+        remappings=[
+            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/output/path", "obstacle_avoidance_planner/trajectory"),
+        ],
+        parameters=[
+            nearest_search_param,
+            path_sampler_param,
+            vehicle_info_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # obstacle velocity limiter
     with open(
         LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r"
@@ -209,7 +230,6 @@ def launch_setup(context, *args, **kwargs):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
-            obstacle_avoidance_planner_component,
             obstacle_velocity_limiter_component,
         ],
     )
@@ -232,6 +252,18 @@ def launch_setup(context, *args, **kwargs):
         condition=LaunchConfigurationEquals("cruise_planner_type", "none"),
     )
 
+    obstacle_avoidance_planner_loader = LoadComposableNodes(
+        composable_node_descriptions=[obstacle_avoidance_planner_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_planner_type", "obstacle_avoidance_planner"),
+    )
+
+    path_sampler_loader = LoadComposableNodes(
+        composable_node_descriptions=[path_sampler_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_planner_type", "path_sampler"),
+    )
+
     surround_obstacle_checker_loader = LoadComposableNodes(
         composable_node_descriptions=[surround_obstacle_checker_component],
         target_container=container,
@@ -241,6 +273,8 @@ def launch_setup(context, *args, **kwargs):
     group = GroupAction(
         [
             container,
+            obstacle_avoidance_planner_loader,
+            path_sampler_loader,
             obstacle_stop_planner_loader,
             obstacle_cruise_planner_loader,
             obstacle_cruise_planner_relay_loader,
@@ -273,6 +307,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg(
         "cruise_planner_type"
     )  # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none"
+    add_launch_arg(
+        "path_planner_type", "obstacle_avoidance_planner"
+    )  # select from "obstacle_avoidance_planner", "path_sampler"
 
     add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -37,7 +37,8 @@
       enable_cancel_lane_change: true
       enable_abort_lane_change: false
 
-      abort_delta_time: 3.0                      # [s]
+      abort_delta_time: 1.0                      # [s]
+      aborting_time: 5.0                         # [s]
       abort_max_lateral_jerk: 1000.0             # [m/s3]
 
       # debug

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -205,7 +205,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
-  void requestApproval();
+  void decideVelocity();
 
   // validation
   bool hasEnoughDistance(const PullOverPath & pull_over_path) const;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -22,6 +22,7 @@
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
+#include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp"
@@ -30,8 +31,6 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
-#include <tf2/utils.h>
-
 #include <algorithm>
 #include <memory>
 #include <string>
@@ -41,6 +40,7 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
@@ -73,17 +73,31 @@ class LaneChangeBase
 
   virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0;
 
+  virtual PathWithLaneId getReferencePath() const = 0;
+
+  virtual void resetParameters() = 0;
+
+  virtual TurnSignalInfo updateOutputTurnSignal() = 0;
+
   virtual bool hasFinishedLaneChange() const = 0;
 
-  virtual PathWithLaneId getReferencePath() const = 0;
+  virtual bool hasFinishedAbort() const = 0;
 
-  virtual bool isCancelConditionSatisfied() = 0;
+  virtual bool isAbortState() const = 0;
 
-  virtual bool isAbortConditionSatisfied(const Pose & pose) = 0;
+  virtual bool isAbleToReturnCurrentLane() const = 0;
 
-  virtual void resetParameters() = 0;
+  virtual LaneChangePath getLaneChangePath() const = 0;
 
-  virtual TurnSignalInfo updateOutputTurnSignal() = 0;
+  virtual bool isEgoOnPreparePhase() const = 0;
+
+  virtual bool isRequiredStop(const bool is_object_coming_from_rear) const = 0;
+
+  virtual PathSafetyStatus isApprovedPathSafe() const = 0;
+
+  virtual bool isNearEndOfLane() const = 0;
+
+  virtual bool getAbortPath() = 0;
 
   virtual void setPreviousModulePaths(
     const std::shared_ptr<PathWithLaneId> & prev_module_reference_path,
@@ -111,63 +125,45 @@ class LaneChangeBase
 
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
 
-  LaneChangePath getLaneChangePath() const
-  {
-    return isAbortState() ? *abort_path_ : status_.lane_change_path;
-  }
-
   const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; }
 
   const CollisionCheckDebugMap & getDebugData() const { return object_debug_; }
 
-  bool isAbortState() const
-  {
-    if (!lane_change_parameters_->enable_abort_lane_change) {
-      return false;
-    }
+  const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
-    const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane(
-      status_.current_lanes, getEgoPose(), planner_data_->parameters);
+  const Point & getEgoPosition() const { return getEgoPose().position; }
 
-    if (!is_within_current_lane) {
-      return false;
-    }
+  const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; }
 
-    if (current_lane_change_state_ != LaneChangeStates::Abort) {
-      return false;
-    }
+  const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; }
 
-    if (!abort_path_) {
-      return false;
-    }
+  bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; }
 
-    return true;
-  }
+  bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; }
 
   bool isSafe() const { return status_.is_safe; }
 
   bool isStopState() const { return current_lane_change_state_ == LaneChangeStates::Stop; }
 
   bool isValidPath() const { return status_.is_valid_path; }
 
-  bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; }
+  void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
 
-  std_msgs::msg::Header getRouteHeader() const
-  {
-    return planner_data_->route_handler->getRouteHeader();
-  }
+  void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
 
-  void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
+  void toStopState() { current_lane_change_state_ = LaneChangeStates::Stop; }
 
-  const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
+  void toCancelState() { current_lane_change_state_ = LaneChangeStates::Cancel; }
 
-  const Point & getEgoPosition() const { return getEgoPose().position; }
+  void toAbortState() { current_lane_change_state_ = LaneChangeStates::Abort; }
 
-  const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; }
+  double getEgoVelocity() const { return getEgoTwist().linear.x; }
 
   std::shared_ptr<RouteHandler> getRouteHandler() const { return planner_data_->route_handler; }
 
-  double getEgoVelocity() const { return getEgoTwist().linear.x; }
+  std_msgs::msg::Header getRouteHeader() const { return getRouteHandler()->getRouteHeader(); }
+
+  std::string getModuleTypeStr() const { return std::string{magic_enum::enum_name(type_)}; }
 
   Direction getDirection() const
   {
@@ -196,33 +192,15 @@ class LaneChangeBase
 
   virtual std::vector<DrivableLanes> getDrivableLanes() const = 0;
 
-  virtual bool isApprovedPathSafe(Pose & ego_pose_before_collision) const = 0;
-
   virtual void calcTurnSignalInfo() = 0;
 
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 
+  virtual bool isAbleToStopSafely() const = 0;
+
   virtual lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0;
 
-  bool isNearEndOfLane() const
-  {
-    const auto & current_pose = getEgoPose();
-    const auto shift_intervals = planner_data_->route_handler->getLateralIntervalsToPreferredLane(
-      status_.current_lanes.back());
-    const double threshold =
-      utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals);
-
-    return (std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) -
-            threshold) < planner_data_->parameters.backward_length_buffer_for_end_of_lane;
-  }
-
-  bool isCurrentSpeedLow() const
-  {
-    constexpr double threshold_ms = 10.0 * 1000 / 3600;
-    return getEgoTwist().linear.x < threshold_ms;
-  }
-
   LaneChangeStatus status_{};
   PathShifter path_shifter_{};
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp

@@ -126,8 +126,6 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
     const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
 
-  ModuleStatus updateState() override;
-
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };
@@ -168,8 +166,6 @@ class LaneChangeBTModule : public LaneChangeBTInterface
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<LaneChangeParameters> & parameters);
 
-  ModuleStatus updateState() override;
-
 protected:
   void updateRTCStatus(const double start_distance, const double finish_distance) override;
 };

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -49,21 +49,37 @@ class NormalLaneChange : public LaneChangeBase
 
   std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;
 
+  LaneChangePath getLaneChangePath() const override;
+
   BehaviorModuleOutput generateOutput() override;
 
   void extendOutputDrivableArea(BehaviorModuleOutput & output) override;
 
+  PathWithLaneId getReferencePath() const override;
+
+  void resetParameters() override;
+
+  TurnSignalInfo updateOutputTurnSignal() override;
+
+  bool getAbortPath() override;
+
+  PathSafetyStatus isApprovedPathSafe() const override;
+
+  bool isRequiredStop(const bool is_object_coming_from_rear) const override;
+
+  bool isNearEndOfLane() const override;
+
   bool hasFinishedLaneChange() const override;
 
-  PathWithLaneId getReferencePath() const override;
+  bool isAbleToReturnCurrentLane() const override;
 
-  bool isCancelConditionSatisfied() override;
+  bool isEgoOnPreparePhase() const override;
 
-  bool isAbortConditionSatisfied(const Pose & pose) override;
+  bool isAbleToStopSafely() const override;
 
-  void resetParameters() override;
+  bool hasFinishedAbort() const override;
 
-  TurnSignalInfo updateOutputTurnSignal() override;
+  bool isAbortState() const override;
 
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
@@ -85,8 +101,6 @@ class NormalLaneChange : public LaneChangeBase
 
   std::vector<DrivableLanes> getDrivableLanes() const override;
 
-  bool isApprovedPathSafe(Pose & ego_pose_before_collision) const override;
-
   void calcTurnSignalInfo() override;
 
   bool isValidPath(const PathWithLaneId & path) const override;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -53,7 +53,8 @@ struct LaneChangeParameters
   bool enable_cancel_lane_change{true};
   bool enable_abort_lane_change{false};
 
-  double abort_delta_time{3.0};
+  double abort_delta_time{1.0};
+  double aborting_time{5.0};
   double abort_max_lateral_jerk{10.0};
 
   // debug marker
@@ -104,4 +105,13 @@ struct AvoidanceByLCParameters
 };
 }  // namespace behavior_path_planner
 
+namespace behavior_path_planner::data::lane_change
+{
+struct PathSafetyStatus
+{
+  bool is_safe{true};
+  bool is_object_coming_from_rear{false};
+};
+}  // namespace behavior_path_planner::data::lane_change
+
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_