feat(lane_change): modify lane change target boundary check to consid…

…er velocity (autowarefoundation#8961)

* check if candidate path footprint exceeds target lane boundary when lc velocity is above minimum

Signed-off-by: mohammad alqudah <[email protected]>

* move functions to relevant module

Signed-off-by: mohammad alqudah <[email protected]>

* suppress unused function cppcheck

Signed-off-by: mohammad alqudah <[email protected]>

* minor change

Signed-off-by: mohammad alqudah <[email protected]>

---------

Signed-off-by: mohammad alqudah <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
 {
 }
 
+// cppcheck-suppress unusedFunction
 bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
 {
   return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
          module_type_->isValidPath();
 }
+
+// cppcheck-suppress unusedFunction
 void AvoidanceByLaneChangeInterface::processOnEntry()
 {
   waitApproval();

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -186,8 +186,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);
 }
 
-std::unique_ptr<SceneModuleInterface>
-AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
+// cppcheck-suppress unusedFunction
+SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<AvoidanceByLaneChangeInterface>(
     name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp

@@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager;
 using autoware::behavior_path_planner::LaneChangeModuleType;
 using autoware::behavior_path_planner::SceneModuleInterface;
 
+using SMIPtr = std::unique_ptr<SceneModuleInterface>;
+
 class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
 {
 public:
@@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
 
   void init(rclcpp::Node * node) override;
 
-  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+  SMIPtr createNewSceneModuleInstance() override;
 
 private:
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters_;

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -34,13 +34,55 @@
 #include <optional>
 #include <utility>
 
+namespace
+{
+geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
+{
+  geometry_msgs::msg::Point32 p;
+  p.x = static_cast<float>(pose.position.x);
+  p.y = static_cast<float>(pose.position.y);
+  p.z = static_cast<float>(pose.position.z);
+  return p;
+};
+
+geometry_msgs::msg::Polygon create_execution_area(
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+  const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset)
+{
+  const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
+  const double & width = vehicle_info.vehicle_width_m;
+  const double & base_to_rear = vehicle_info.rear_overhang_m;
+
+  // if stationary object, extend forward and backward by the half of lon length
+  const double forward_lon_offset = base_to_front + additional_lon_offset;
+  const double backward_lon_offset = -base_to_rear;
+  const double lat_offset = width / 2.0 + additional_lat_offset;
+
+  const auto p1 =
+    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
+  const auto p2 =
+    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
+  const auto p3 =
+    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
+  const auto p4 =
+    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
+  geometry_msgs::msg::Polygon polygon;
+
+  polygon.points.push_back(create_point32(p1));
+  polygon.points.push_back(create_point32(p2));
+  polygon.points.push_back(create_point32(p3));
+  polygon.points.push_back(create_point32(p4));
+
+  return polygon;
+}
+}  // namespace
+
 namespace autoware::behavior_path_planner
 {
 using autoware::behavior_path_planner::Direction;
 using autoware::behavior_path_planner::LaneChangeModuleType;
 using autoware::behavior_path_planner::ObjectInfo;
 using autoware::behavior_path_planner::Point2d;
-using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea;
 
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
@@ -84,7 +126,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
   const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
   const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
 
-  lane_change_debug_.execution_area = createExecutionArea(
+  lane_change_debug_.execution_area = create_execution_area(
     getCommonParam().vehicle_info, getEgoPose(),
     std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -91,7 +91,7 @@ bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
 
-bool pathFootprintExceedsTargetLaneBound(
+bool path_footprint_exceeds_target_lane_bound(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
   const double margin = 0.1);
 
@@ -257,10 +257,6 @@ bool is_same_lane_with_prev_iteration(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
 
-Pose to_pose(
-  const autoware::universe_utils::Point2d & point,
-  const geometry_msgs::msg::Quaternion & orientation);
-
 bool is_ahead_of_ego(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
   const PredictedObject & object);
@@ -280,13 +276,4 @@ double get_distance_to_next_regulatory_element(
   const bool ignore_intersection = false);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
-namespace autoware::behavior_path_planner::utils::lane_change::debug
-{
-geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose);
-
-geometry_msgs::msg::Polygon createExecutionArea(
-  const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset,
-  double additional_lat_offset);
-}  // namespace autoware::behavior_path_planner::utils::lane_change::debug
-
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1609,6 +1609,18 @@ bool NormalLaneChange::check_candidate_path_safety(
       "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change.");
   }
 
+  const auto lc_start_velocity = candidate_path.info.velocity.prepare;
+  const auto min_lc_velocity = lane_change_parameters_->minimum_lane_changing_velocity;
+  constexpr double margin = 0.1;
+  // path is unsafe if it exceeds target lane boundary with a high velocity
+  if (
+    lane_change_parameters_->enable_target_lane_bound_check &&
+    lc_start_velocity > min_lc_velocity + margin &&
+    utils::lane_change::path_footprint_exceeds_target_lane_bound(
+      common_data_ptr_, candidate_path.shifted_path.path, planner_data_->parameters.vehicle_info)) {
+    throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change.");
+  }
+
   constexpr size_t decel_sampling_num = 1;
   const auto safety_check_with_normal_rss = isLaneChangePathSafe(
     candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, decel_sampling_num,

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -200,8 +200,7 @@ bool isPathInLanelets(
   return true;
 }
 
-// cppcheck-suppress unusedFunction
-bool pathFootprintExceedsTargetLaneBound(
+bool path_footprint_exceeds_target_lane_bound(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
   const double margin)
 {
@@ -1145,16 +1144,6 @@ bool is_same_lane_with_prev_iteration(
          (prev_target_lanes.back().id() == prev_target_lanes.back().id());
 }
 
-Pose to_pose(
-  const autoware::universe_utils::Point2d & point,
-  const geometry_msgs::msg::Quaternion & orientation)
-{
-  Pose pose;
-  pose.position = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0);
-  pose.orientation = orientation;
-  return pose;
-}
-
 bool is_ahead_of_ego(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
   const PredictedObject & object)
@@ -1279,47 +1268,3 @@ double get_distance_to_next_regulatory_element(
   return distance;
 }
 }  // namespace autoware::behavior_path_planner::utils::lane_change
-
-namespace autoware::behavior_path_planner::utils::lane_change::debug
-{
-geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
-{
-  geometry_msgs::msg::Point32 p;
-  p.x = static_cast<float>(pose.position.x);
-  p.y = static_cast<float>(pose.position.y);
-  p.z = static_cast<float>(pose.position.z);
-  return p;
-};
-
-geometry_msgs::msg::Polygon createExecutionArea(
-  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose,
-  double additional_lon_offset, double additional_lat_offset)
-{
-  const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
-  const double & width = vehicle_info.vehicle_width_m;
-  const double & base_to_rear = vehicle_info.rear_overhang_m;
-
-  // if stationary object, extend forward and backward by the half of lon length
-  const double forward_lon_offset = base_to_front + additional_lon_offset;
-  const double backward_lon_offset = -base_to_rear;
-  const double lat_offset = width / 2.0 + additional_lat_offset;
-
-  const auto p1 =
-    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
-  const auto p2 =
-    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
-  const auto p3 =
-    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
-  const auto p4 =
-    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
-  geometry_msgs::msg::Polygon polygon;
-
-  polygon.points.push_back(create_point32(p1));
-  polygon.points.push_back(create_point32(p2));
-  polygon.points.push_back(create_point32(p3));
-  polygon.points.push_back(create_point32(p4));
-
-  return polygon;
-}
-
-}  // namespace autoware::behavior_path_planner::utils::lane_change::debug