Merge pull request #1720 from tier4/cp-lane-change-stability

perf(lane_change): path stability and correctness

planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp

@@ -324,6 +324,9 @@ class RouteHandler
   lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
 
+  Pose get_pose_from_2d_arc_length(
+    const lanelet::ConstLanelets & lanelet_sequence, const double s) const;
+
 private:
   // MUST
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;

planning/autoware_route_handler/src/route_handler.cpp

@@ -46,6 +46,8 @@ namespace autoware::route_handler
 {
 namespace
 {
+using autoware::universe_utils::createPoint;
+using autoware::universe_utils::createQuaternionFromYaw;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::Path;
 using geometry_msgs::msg::Pose;
@@ -2225,4 +2227,29 @@ std::optional<lanelet::routing::LaneletPath> RouteHandler::findDrivableLanePath(
   if (route) return route->shortestPath();
   return {};
 }
+
+Pose RouteHandler::get_pose_from_2d_arc_length(
+  const lanelet::ConstLanelets & lanelet_sequence, const double s) const
+{
+  double accumulated_distance2d = 0;
+  for (const auto & llt : lanelet_sequence) {
+    const auto & centerline = llt.centerline();
+    for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) {
+      const auto pt = *it;
+      const auto next_pt = *std::next(it);
+      const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt));
+      if (accumulated_distance2d + distance2d > s) {
+        const double ratio = (s - accumulated_distance2d) / distance2d;
+        const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio;
+        const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x());
+        Pose pose;
+        pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z());
+        pose.orientation = createQuaternionFromYaw(yaw);
+        return pose;
+      }
+      accumulated_distance2d += distance2d;
+    }
+  }
+  return Pose{};
+}
 }  // namespace autoware::route_handler

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

@@ -187,8 +187,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,14 +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;
-namespace utils = autoware::behavior_path_planner::utils;
 
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
@@ -83,9 +124,9 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 
   const auto & nearest_object = data.target_objects.front();
   const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
+  const auto minimum_lane_change_length = calc_minimum_dist_buffer();
 
-  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());
 
@@ -274,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
   return avoidance_helper_->getMinAvoidanceDistance(shift_length);
 }
 
-double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
+double AvoidanceByLaneChange::calc_minimum_dist_buffer() const
 {
-  const auto current_lanes = get_current_lanes();
-  if (current_lanes.empty()) {
-    RCLCPP_DEBUG(logger_, "no empty lanes");
-    return std::numeric_limits<double>::infinity();
-  }
-
-  return utils::lane_change::calculation::calc_minimum_lane_change_length(
-    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
+  const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
+    common_data_ptr_, get_current_lanes());
+  return dist_buffer.min;
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp

@@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
   void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
 
   double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
-  double calcMinimumLaneChangeLength() const;
+  double calc_minimum_dist_buffer() const;
   double calcLateralOffset() const;
 };
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt

@@ -18,6 +18,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}
     test/test_behavior_path_planner_node_interface.cpp
     test/test_lane_change_utils.cpp
+    # test/test_lane_change_scene.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}