fix(behavior_path_planner): lane change ego and target polygon pose (a…

…utowarefoundation#1786)

* fix(behavior_path_planner): lane change ego and target polygon pose

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* reduce lane changing duration

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor(behavior_path_planner): lane change refactoring

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

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

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_
 
 #include "behavior_path_planner/scene_module/lane_change/debug.hpp"
+#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 
@@ -36,38 +37,6 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using marker_utils::CollisionCheckDebug;
 
-struct LaneChangeParameters
-{
-  double lane_change_prepare_duration;
-  double lane_changing_duration;
-  double minimum_lane_change_prepare_distance;
-  double lane_change_finish_judge_buffer;
-  double minimum_lane_change_velocity;
-  double prediction_time_resolution;
-  double maximum_deceleration;
-  int lane_change_sampling_num;
-  double abort_lane_change_velocity_thresh;
-  double abort_lane_change_angle_thresh;
-  double abort_lane_change_distance_thresh;
-  bool enable_abort_lane_change;
-  bool enable_collision_check_at_prepare_phase;
-  bool use_predicted_path_outside_lanelet;
-  bool use_all_predicted_path;
-  bool publish_debug_marker;
-};
-
-struct LaneChangeStatus
-{
-  PathWithLaneId lane_follow_path;
-  LaneChangePath lane_change_path;
-  lanelet::ConstLanelets current_lanes;
-  lanelet::ConstLanelets lane_change_lanes;
-  std::vector<uint64_t> lane_follow_lane_ids;
-  std::vector<uint64_t> lane_change_lane_ids;
-  bool is_safe;
-  double start_distance;
-};
-
 class LaneChangeModule : public SceneModuleInterface
 {
 public:
@@ -159,7 +128,6 @@ class LaneChangeModule : public SceneModuleInterface
   }
 
   PathWithLaneId getReferencePath() const;
-  lanelet::ConstLanelets getCurrentLanes() const;
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const;
   std::pair<bool, bool> getSafePath(

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

@@ -0,0 +1,59 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_
+
+#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
+#include "lanelet2_core/geometry/Lanelet.h"
+
+#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp"
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+struct LaneChangeParameters
+{
+  double lane_change_prepare_duration;
+  double lane_changing_duration;
+  double minimum_lane_change_prepare_distance;
+  double lane_change_finish_judge_buffer;
+  double minimum_lane_change_velocity;
+  double prediction_time_resolution;
+  double maximum_deceleration;
+  int lane_change_sampling_num;
+  double abort_lane_change_velocity_thresh;
+  double abort_lane_change_angle_thresh;
+  double abort_lane_change_distance_thresh;
+  bool enable_abort_lane_change;
+  bool enable_collision_check_at_prepare_phase;
+  bool use_predicted_path_outside_lanelet;
+  bool use_all_predicted_path;
+  bool publish_debug_marker;
+};
+
+struct LaneChangeStatus
+{
+  PathWithLaneId lane_follow_path;
+  LaneChangePath lane_change_path;
+  lanelet::ConstLanelets current_lanes;
+  lanelet::ConstLanelets lane_change_lanes;
+  std::vector<uint64_t> lane_follow_lane_ids;
+  std::vector<uint64_t> lane_change_lane_ids;
+  bool is_safe;
+  double start_distance;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_

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

@@ -19,6 +19,8 @@
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <vector>
+
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -31,5 +33,7 @@ struct LaneChangePath
   double preparation_length{0.0};
   double lane_change_length{0.0};
 };
+using LaneChangePaths = std::vector<LaneChangePath>;
+
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_

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

@@ -43,47 +43,89 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using tier4_autoware_utils::Polygon2d;
 
-PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2);
+PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
+
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
+
 double getExpectedVelocityWhenDecelerate(
   const double & current_velocity, const double & expected_acceleration,
   const double & lane_change_prepare_duration);
+
 double getDistanceWhenDecelerate(
   const double & velocity, const double & expected_acceleration, const double & duration,
   const double & minimum_distance);
-std::vector<LaneChangePath> getLaneChangePaths(
+
+std::optional<LaneChangePath> constructCandidatePath(
+  const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
+  const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point,
+  const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
+  const double & acceleration, const double & prepare_distance, const double & lane_change_distance,
+  const double & lane_changing_duration, const double & minimum_lane_change_velocity);
+
+LaneChangePaths getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
-  const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter);
-std::vector<LaneChangePath> selectValidPaths(
-  const std::vector<LaneChangePath> & paths, const lanelet::ConstLanelets & current_lanes,
+  const BehaviorPathPlannerParameters & common_parameter,
+  const behavior_path_planner::LaneChangeParameters & parameter);
+
+LaneChangePaths selectValidPaths(
+  const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose,
   const bool isInGoalRouteSection, const Pose & goal_pose);
+
 bool selectSafePath(
-  const std::vector<LaneChangePath> & paths, const lanelet::ConstLanelets & current_lanes,
+  const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
   const behavior_path_planner::LaneChangeParameters & ros_parameters,
   LaneChangePath * selected_path,
   std::unordered_map<std::string, CollisionCheckDebug> & debug_data);
+
 bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const size_t current_seg_idx, const Twist & current_twist,
+  const size_t & current_seg_idx, const Twist & current_twist,
   const BehaviorPathPlannerParameters & common_parameters,
   const behavior_path_planner::LaneChangeParameters & lane_change_parameters,
   std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const bool use_buffer = true,
   const double acceleration = 0.0);
+
 bool hasEnoughDistance(
   const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
   const bool isInGoalRouteSection, const Pose & goal_pose,
   const lanelet::routing::RoutingGraphContainer & overall_graphs);
+
+ShiftPoint getLaneChangeShiftPoint(
+  const PathWithLaneId & path1, const PathWithLaneId & path2,
+  const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path);
+
+PathWithLaneId getReferencePathFromTargetLane(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
+  const Pose & lane_changing_start_pose, const double & prepare_distance,
+  const double & lane_changing_distance, const double & forward_path_length);
+
+PathWithLaneId getReferencePathFromTargetLane(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
+  const Pose & in_target_front_pose, const Pose & in_target_end_pose);
+
+PathWithLaneId getLaneChangePathPrepareSegment(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
+  const Pose & current_pose, const double & backward_path_length, const double & prepare_distance,
+  const double & prepare_duration, const double & minimum_lane_change_velocity);
+
+PathWithLaneId getLaneChangePathLaneChangingSegment(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
+  const Pose & current_pose, const double & forward_path_length, const double & prepare_distance,
+  const double & lane_change_distance, const double & minimum_lane_change_length,
+  const double & lane_change_distance_buffer, const double & lane_changing_duration,
+  const double & minimum_lane_change_velocity);
+
 }  // namespace behavior_path_planner::lane_change_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -74,6 +74,7 @@ using tier4_autoware_utils::Polygon2d;
 namespace bg = boost::geometry;
 using geometry_msgs::msg::Pose;
 using marker_utils::CollisionCheckDebug;
+using vehicle_info_util::VehicleInfo;
 
 struct FrenetCoordinate3d
 {
@@ -381,8 +382,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
   const double forward_length, const double backward_length);
+
 Polygon2d convertBoundingBoxObjectToGeometryPolygon(
-  const Pose & current_pose, const double & length, const double & width);
+  const Pose & current_pose, const double & base_to_front, const double & base_to_rear,
+  const double & base_to_width);
 
 Polygon2d convertCylindricalObjectToGeometryPolygon(
   const Pose & current_pose, const Shape & obj_shape);
@@ -399,7 +402,7 @@ Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target
 bool getEgoExpectedPoseAndConvertToPolygon(
   const Pose & current_pose, const PredictedPath & pred_path, Pose & expected_pose,
   tier4_autoware_utils::Polygon2d & ego_polygon, const double & check_current_time,
-  const double & length, const double & width);
+  const VehicleInfo & ego_info);
 
 bool getObjectExpectedPoseAndConvertToPolygon(
   const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose,
@@ -432,16 +435,16 @@ bool isLateralDistanceEnough(
 
 bool isSafeInLaneletCollisionCheck(
   const Pose & ego_current_pose, const Twist & ego_current_twist,
-  const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
-  const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
+  const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
+  const double & check_start_time, const double & check_end_time,
   const double & check_time_resolution, const PredictedObject & target_object,
   const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters,
   CollisionCheckDebug & debug);
 
 bool isSafeInFreeSpaceCollisionCheck(
   const Pose & ego_current_pose, const Twist & ego_current_twist,
-  const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
-  const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
+  const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info,
+  const double & check_start_time, const double & check_end_time,
   const double & check_time_resolution, const PredictedObject & target_object,
   const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug);
 }  // namespace behavior_path_planner::util

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

@@ -51,7 +51,8 @@ MarkerArray showObjectInfo(
     std::ostringstream ss;
 
     ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x
-       << "\nLat: " << info.relative_to_ego.position.y;
+       << "\nLat: " << info.relative_to_ego.position.y
+       << "\nPosition: " << (info.is_front ? "front" : "back");
 
     marker.text = ss.str();