feat(behavior_path_planner): stabilize lane_following end points (#1632)

* feat(behavior_path_planner): stabilize lane_following end points

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp

@@ -32,10 +32,6 @@ double calcShiftLength(
 
 bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
 
-lanelet::ConstLanelets calcLaneAroundPose(
-  const std::shared_ptr<const PlannerData> & planner_data, const Pose & pose,
-  const double backward_length);
-
 size_t findPathIndexFromArclength(
   const std::vector<double> & path_arclength_arr, const double target_arc);
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -282,6 +282,10 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr<const PlannerData>
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets calcLaneAroundPose(
+  const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
+  const double forward_length, const double backward_length);
+
 }  // namespace behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -148,8 +148,9 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
     calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0));
 
   // lanelet info
-  data.current_lanelets = calcLaneAroundPose(
-    planner_data_, reference_pose.pose, planner_data_->parameters.backward_path_length);
+  data.current_lanelets = util::calcLaneAroundPose(
+    planner_data_->route_handler, reference_pose.pose,
+    planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length);
 
   // target objects for avoidance
   data.objects = calcAvoidanceTargetObjects(data.current_lanelets, data.reference_path, debug);
@@ -1875,7 +1876,7 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath(
     p.backward_path_length, longest_dist_to_shift_point, backward_length);
 
   const lanelet::ConstLanelets current_lanes =
-    calcLaneAroundPose(planner_data, pose.pose, backward_length);
+    util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length);
   centerline_path = util::getCenterLinePath(
     *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p);
 

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp

@@ -43,28 +43,6 @@ bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_
   return (is_object_on_right == std::signbit(shift_length));
 }
 
-lanelet::ConstLanelets calcLaneAroundPose(
-  const std::shared_ptr<const PlannerData> & planner_data, const geometry_msgs::msg::Pose & pose,
-  const double backward_length)
-{
-  const auto & p = planner_data->parameters;
-  const auto & route_handler = planner_data->route_handler;
-
-  lanelet::ConstLanelet current_lane;
-  if (!route_handler->getClosestLaneletWithinRoute(pose, &current_lane)) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
-      "failed to find closest lanelet within route!!!");
-    return {};  // TODO(Horibe)
-  }
-
-  // For current_lanes with desired length
-  lanelet::ConstLanelets current_lanes =
-    route_handler->getLaneletSequence(current_lane, pose, backward_length, p.forward_path_length);
-
-  return current_lanes;
-}
-
 ShiftedPath toShiftedPath(const PathWithLaneId & path)
 {
   ShiftedPath out;

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/scene_module/lane_following/lane_following_module.hpp"
 
+#include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -99,8 +100,18 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
     return reference_path;
   }
 
+  // calculate path with backward margin to avoid end points' instability by spline interpolation
+  constexpr double extra_margin = 10.0;
+  const double backward_length =
+    std::max(p.backward_path_length, p.backward_path_length + extra_margin);
+  const auto current_lanes_with_backward_margin =
+    util::calcLaneAroundPose(route_handler, current_pose, p.forward_path_length, backward_length);
   reference_path = util::getCenterLinePath(
-    *route_handler, current_lanes, current_pose, p.backward_path_length, p.forward_path_length, p);
+    *route_handler, current_lanes_with_backward_margin, current_pose, backward_length,
+    p.forward_path_length, p);
+
+  // clip backward length
+  util::clipPathLength(reference_path, current_pose, p.forward_path_length, p.backward_path_length);
 
   {
     double optional_lengths{0.0};
@@ -148,5 +159,4 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
 
   return reference_path;
 }
-
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -24,28 +24,6 @@
 #include <memory>
 #include <string>
 
-namespace
-{
-lanelet::ConstLanelets calcLaneAroundPose(
-  const std::shared_ptr<const behavior_path_planner::PlannerData> & planner_data,
-  const geometry_msgs::msg::Pose & pose, const double backward_length)
-{
-  const auto & p = planner_data->parameters;
-  const auto & route_handler = planner_data->route_handler;
-
-  lanelet::ConstLanelet current_lane;
-  if (!route_handler->getClosestLaneletWithinRoute(pose, &current_lane)) {
-    return {};  // TODO(Horibe)
-  }
-
-  // For current_lanes with desired length
-  lanelet::ConstLanelets current_lanes =
-    route_handler->getLaneletSequence(current_lane, pose, backward_length, p.forward_path_length);
-
-  return current_lanes;
-}
-}  // namespace
-
 namespace behavior_path_planner
 {
 using geometry_msgs::msg::Point;
@@ -488,7 +466,7 @@ PathWithLaneId SideShiftModule::calcCenterLinePath(
     p.backward_path_length, longest_dist_to_shift_point, backward_length);
 
   const lanelet::ConstLanelets current_lanes =
-    calcLaneAroundPose(planner_data, pose.pose, backward_length);
+    util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length);
   centerline_path = util::getCenterLinePath(
     *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p);
 

planning/behavior_path_planner/src/utilities.cpp

@@ -1978,4 +1978,22 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   return current_lanes;
 }
 
+lanelet::ConstLanelets calcLaneAroundPose(
+  const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
+  const double forward_length, const double backward_length)
+{
+  lanelet::ConstLanelet current_lane;
+  if (!route_handler->getClosestLaneletWithinRoute(pose, &current_lane)) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
+      "failed to find closest lanelet within route!!!");
+    return {};  // TODO(Horibe)
+  }
+
+  // For current_lanes with desired length
+  lanelet::ConstLanelets current_lanes =
+    route_handler->getLaneletSequence(current_lane, pose, backward_length, forward_length);
+
+  return current_lanes;
+}
 }  // namespace behavior_path_planner::util