Merge pull request autowarefoundation#869 from tier4/fix/lane_change

feat(behavior_path_planner): add virtual stop wall for lane change

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

@@ -209,6 +209,10 @@ class LaneChangeBase
     return direction_;
   }
 
+  boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }
+
+  void resetStopPose() { lane_change_stop_pose_ = boost::none; }
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 
@@ -244,6 +248,7 @@ class LaneChangeBase
   PathWithLaneId prev_module_path_{};
   DrivableAreaInfo prev_drivable_area_info_{};
   TurnSignalInfo prev_turn_signal_info_{};
+  boost::optional<Pose> lane_change_stop_pose_{boost::none};
 
   PathWithLaneId prev_approved_path_{};
 

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

@@ -153,6 +153,8 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
+  void setStopPose(const Pose & stop_pose);
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -203,6 +203,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModulePaths(
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateSpecialData();
+  module_type_->resetStopPose();
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()
@@ -221,6 +222,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   *prev_approved_path_ = *getPreviousModuleOutput().path;
   module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path);
 
+  stop_pose_ = module_type_->getStopPose();
+
   updateSteeringFactorPtr(output);
   clearWaitingApproval();
 
@@ -249,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   path_reference_ = getPreviousModuleOutput().reference_path;
 
+  stop_pose_ = module_type_->getStopPose();
+
   if (!module_type_->isValidPath()) {
     removeRTCStatus();
     path_candidate_ = std::make_shared<PathWithLaneId>();
@@ -286,6 +291,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters)
 
 void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & data)
 {
+  planner_data_ = data;
   module_type_->setData(data);
 }
 

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -138,6 +138,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
     const auto stop_dist =
       -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
     const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
+    setStopPose(stop_point.point.pose);
   }
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
@@ -198,6 +199,7 @@ void NormalLaneChange::insertStopPoint(
   const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
   if (stopping_distance > 0.0) {
     const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
   }
 }
 
@@ -649,7 +651,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const lanelet::ConstLanelets & target_backward_lanes) const
 {
   const auto current_pose = getEgoPose();
-  const auto current_vel = getEgoVelocity();
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   const auto & objects = *planner_data_->dynamic_object;
@@ -672,12 +673,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const auto target_backward_polygon = utils::lane_change::createPolygon(
     target_backward_lanes, 0.0, std::numeric_limits<double>::max());
 
-  // minimum distance to lane changing start point
-  const double t_prepare = common_parameters.lane_change_prepare_duration;
-  const double a_min = lane_change_parameters_->min_longitudinal_acc;
-  const double min_dist_to_lane_changing_start = std::max(
-    current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare);
-
   auto filtered_objects = objects;
 
   utils::path_safety_checker::filterObjectsByClass(
@@ -712,12 +707,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
 
     // check if the object intersects with target lanes
     if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
-      // ignore static parked object that are in front of the ego vehicle in target lanes
-      bool is_parked_object =
-        utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0);
-      if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) {
-        continue;
-      }
+      // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
+      // lanes
 
       filtered_obj_indices.target_lane.push_back(i);
       continue;
@@ -1466,4 +1457,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
   return path_safety_status;
 }
+
+void NormalLaneChange::setStopPose(const Pose & stop_pose)
+{
+  lane_change_stop_pose_ = stop_pose;
+}
+
 }  // namespace behavior_path_planner