From e4f113147e42f0e6d6b8a6263624a12fb7c4eaf4 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 20 Jun 2022 23:09:53 +0900 Subject: [PATCH 1/3] feat(behavior_path_planner): revise lane change module Signed-off-by: Muhammad Zulfaqar Azmi Refactoring and renaming Remove some of the unused variables Signed-off-by: Muhammad Zulfaqar Azmi Add longitudinal threshold and modify default param Signed-off-by: Muhammad Zulfaqar Azmi fix error after rebase Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 7 + .../config/lane_change/lane_change.param.yaml | 8 +- .../behavior_path_planner/parameters.hpp | 8 + .../lane_change/lane_change_module.hpp | 9 +- .../scene_module/lane_change/util.hpp | 28 +- .../behavior_path_planner/utilities.hpp | 82 ++- .../src/behavior_path_planner_node.cpp | 15 +- .../lane_change/lane_change_module.cpp | 73 +-- .../src/scene_module/lane_change/util.cpp | 250 ++++------ .../behavior_path_planner/src/utilities.cpp | 466 +++++++++++++++--- .../test/test_lane_change_utils.cpp | 37 ++ 11 files changed, 675 insertions(+), 308 deletions(-) create mode 100644 planning/behavior_path_planner/test/test_lane_change_utils.cpp diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index f0889cc638c02..0b643151aa03b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -20,3 +20,10 @@ path_interval: 2.0 visualize_drivable_area_for_shared_linestrings_lanelet: true + + lateral_distance_max_threshold: 5.0 + longitudinal_distance_min_threshold: 3.0 + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 2.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 86226a95ce561..0cd8fa4f7d374 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,19 +1,15 @@ /**: ros__parameters: lane_change: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 lane_change_prepare_duration: 4.0 lane_changing_duration: 8.0 lane_change_finish_judge_buffer: 3.0 minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 maximum_deceleration: 1.0 + lane_change_sampling_num: 10 enable_abort_lane_change: false enable_collision_check_at_prepare_phase: false use_predicted_path_outside_lanelet: false use_all_predicted_path: false - enable_blocked_by_obstacle: false + lane_change_search_distance: 30.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 050bbaa616e1c..295b613c03291 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -56,6 +56,14 @@ struct BehaviorPathPlannerParameters // drivable area visualization bool visualize_drivable_area_for_shared_linestrings_lanelet; + + // collision check + double lateral_distance_max_threshold; + double longitudinal_distance_min_threshold; + double expected_front_deceleration; + double expected_rear_deceleration; + double rear_vehicle_reaction_time; + double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 7813661a31f02..7d4c7c4d8174c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -34,16 +34,12 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct LaneChangeParameters { - double min_stop_distance; - double stop_time; - double hysteresis_buffer_distance; 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_duration; double prediction_time_resolution; - double static_obstacle_velocity_thresh; double maximum_deceleration; int lane_change_sampling_num; double abort_lane_change_velocity_thresh; @@ -53,7 +49,7 @@ struct LaneChangeParameters bool enable_collision_check_at_prepare_phase; bool use_predicted_path_outside_lanelet; bool use_all_predicted_path; - bool enable_blocked_by_obstacle; + double lane_change_search_distance; }; struct LaneChangeStatus @@ -167,7 +163,6 @@ class LaneChangeModule : public SceneModuleInterface void updateLaneChangeStatus(); bool isSafe() const; - bool isLaneBlocked(const lanelet::ConstLanelets & lanes) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; bool isAbortConditionSatisfied() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 9846e36e3aaef..251fad73f077f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -31,25 +31,30 @@ #include #include -namespace behavior_path_planner -{ -namespace lane_change_utils +namespace behavior_path_planner::lane_change_utils { +using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_autoware_utils::Polygon2d; 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 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 behavior_path_planner::LaneChangeParameters & parameter); + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter); std::vector selectValidPaths( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -59,23 +64,20 @@ bool selectSafePath( const std::vector & 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 double vehicle_width, - const behavior_path_planner::LaneChangeParameters & ros_parameters, - LaneChangePath * selected_path); + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path); 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 Twist & current_twist, const double vehicle_width, - const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true, + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, 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); -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); -} // namespace lane_change_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index bd88c63163f87..dd7308fd507c6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -54,6 +54,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; + +using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -66,7 +68,10 @@ using geometry_msgs::msg::Vector3; using nav_msgs::msg::OccupancyGrid; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +namespace bg = boost::geometry; +using geometry_msgs::msg::Pose; struct FrenetCoordinate3d { @@ -74,6 +79,20 @@ struct FrenetCoordinate3d double distance{0.0}; // lateral }; +struct ProjectedDistancePoint +{ + Point2d projected_point; + double distance{0.0}; +}; + +template > +ProjectedDistancePoint pointToSegment( + const Point2d & reference_point, const Point2d & point_from_ego, + const Point2d & point_from_object); + +void getProjectedDistancePointFromPolygons( + const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, + Pose & point_on_object); // data conversions Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id); @@ -86,7 +105,8 @@ PoseArray convertToGeometryPoseArray(const PathWithLaneId & path); PredictedPath convertToPredictedPath( const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double duration, const double resolution, const double acceleration); + const double duration, const double resolution, const double acceleration, + double min_speed = 1.0); FrenetCoordinate3d convertToFrenetCoordinate3d( const std::vector & linestring, const Point & search_point_geom); @@ -285,7 +305,67 @@ lanelet::ConstLanelets getExtendedCurrentLanes( lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr 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); + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const Shape & obj_shape); + +Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape); + +std::vector getPredictedPathFromObj( + const PredictedObject & obj, const bool & is_use_all_predicted_path); + +Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object); + +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); + +bool getObjectExpectedPoseAndConvertToPolygon( + const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, + Polygon2d & obj_polygon, const double & check_current_time); + +bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); + +bool isObjectFront(const Pose & projected_ego_pose); + +double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel); + +double frontVehicleStopDistance( + const double & front_vehicle_velocity, const double & front_vehicle_accel, + const double & distance_to_collision); + +double rearVehicleStopDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin); + +bool isLongitudinalDistanceEnough( + const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold); + +bool hasEnoughDistance( + const Pose & expected_ego_pose, const Twist & ego_current_twist, + const Pose & expected_object_pose, const Twist & object_current_twist, + const BehaviorPathPlannerParameters & param); + +bool isLateralDistanceEnough( + const double & relative_lateral_distance, const double & lateral_distance_threshold); + +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 double & check_time_resolution, const PredictedObject & target_object, + const PredictedPath & target_object_path, + const BehaviorPathPlannerParameters & common_parameters); +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 double & check_time_resolution, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d962e36886d35..e04fff4583ebc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -204,6 +204,13 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); + p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0); + p.longitudinal_distance_min_threshold = + declare_parameter("longitudinal_distance_min_threshold", 3.0); + p.expected_front_deceleration = declare_parameter("expected_front_deceleration", -1.0); + p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration", -1.0); + p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time", 2.0); + p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin", 2.0); return p; } @@ -307,16 +314,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() }; LaneChangeParameters p{}; - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_duration = dp("lane_changing_duration", 4.0); + p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 8.3); - p.prediction_duration = dp("prediction_duration", 8.0); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.static_obstacle_velocity_thresh = dp("static_obstacle_velocity_thresh", 0.1); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); p.enable_abort_lane_change = dp("enable_abort_lane_change", true); @@ -327,7 +330,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_lane_change_angle_thresh = dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.enable_blocked_by_obstacle = dp("enable_blocked_by_obstacle", false); + p.lane_change_search_distance = dp("lane_change_search_distance", 30.0); // validation of parameters if (p.lane_change_sampling_num < 1) { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 47c69274564ea..f0f5b932935a4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utilities.hpp" #include @@ -94,9 +95,8 @@ bool LaneChangeModule::isExecutionRequested() const const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); return found_valid_path; @@ -112,13 +112,11 @@ bool LaneChangeModule::isExecutionReady() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); - return found_safe_path && !isLaneBlocked(lane_change_lanes); + return found_safe_path; } BT::NodeStatus LaneChangeModule::updateState() @@ -182,13 +180,15 @@ CandidateOutput LaneChangeModule::planCandidate() const const auto current_lanes = getCurrentLanes(); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + if (selected_path.path.points.empty()) { + return output; + } + const auto start_idx = selected_path.shift_point.start_idx; const auto end_idx = selected_path.shift_point.end_idx; @@ -228,9 +228,8 @@ void LaneChangeModule::updateLaneChangeStatus() status_.lane_change_lanes = lane_change_lanes; // Find lane change path - bool found_valid_path, found_safe_path; LaneChangePath selected_path; - std::tie(found_valid_path, found_safe_path) = + const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); // Update status @@ -390,7 +389,7 @@ std::pair LaneChangeModule::getSafePath( // select safe path bool found_safe_path = lane_change_utils::selectSafePath( valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters.vehicle_width, parameters_, &safe_path); + current_twist, common_parameters, parameters_, &safe_path); return std::make_pair(true, found_safe_path); } @@ -416,54 +415,6 @@ bool LaneChangeModule::isCurrentSpeedLow() const return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; } -bool LaneChangeModule::isLaneBlocked(const lanelet::ConstLanelets & lanes) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - - const auto current_lanes = getCurrentLanes(); - - const auto arc = lanelet::utils::getArcCoordinates(lanes, current_pose); - constexpr double max_check_distance = 100; - double static_obj_velocity_thresh = parameters_.static_obstacle_velocity_thresh; - const double lane_changeable_distance_left = - route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::LEFT); - const double lane_changeable_distance_right = - route_handler->getLaneChangeableDistance(current_pose, LaneChangeDirection::RIGHT); - const double lane_changeable_distance = - std::max(lane_changeable_distance_left, lane_changeable_distance_right); - const double check_distance = std::min(max_check_distance, lane_changeable_distance); - const auto polygon = - lanelet::utils::getPolygonFromArcLength(lanes, arc.length, arc.length + check_distance); - - if (polygon.size() < 3) { - RCLCPP_WARN_STREAM( - getLogger(), "could not get polygon from lanelet with arc lengths: " - << arc.length << " to " << arc.length + check_distance); - return false; - } - - for (const auto & obj : planner_data_->dynamic_object->objects) { - const auto label = util::getHighestProbLabel(obj.classification); - if ( - label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || - label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE) { - const auto velocity = util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - if (velocity < static_obj_velocity_thresh) { - const auto position = lanelet::utils::conversion::toLaneletPoint( - obj.kinematics.initial_pose_with_covariance.pose.position); - const auto distance = boost::geometry::distance( - lanelet::utils::to2D(position).basicPoint(), - lanelet::utils::to2D(polygon).basicPolygon()); - if (distance < std::numeric_limits::epsilon()) { - return true; - } - } - } - } - return false; -} - bool LaneChangeModule::isAbortConditionSatisfied() const { const auto & route_handler = planner_data_->route_handler; @@ -506,7 +457,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const is_path_safe = lane_change_utils::isLaneChangePathSafe( path.path, current_lanes, check_lanes, objects, current_pose, current_twist, - common_parameters.vehicle_width, parameters_, false, status_.lane_change_path.acceleration); + common_parameters, parameters_, false, status_.lane_change_path.acceleration); } // check vehicle velocity thresh diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 250390eafdd67..6108de408472a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" +#include "behavior_path_planner/utilities.hpp" #include #include @@ -30,9 +31,7 @@ #include #include -namespace behavior_path_planner -{ -namespace lane_change_utils +namespace behavior_path_planner::lane_change_utils { PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) { @@ -67,7 +66,19 @@ bool isPathInLanelets( } return true; } +double getExpectedVelocityWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration) +{ + return velocity + expected_acceleration * duration; +} +double getDistanceWhenDecelerate( + const double & velocity, const double & expected_acceleration, const double & duration, + const double & minimum_distance) +{ + const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); + return std::max(distance, minimum_distance); +} std::vector getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, @@ -80,17 +91,19 @@ std::vector getLaneChangePaths( } // rename parameter - const double backward_path_length = common_parameter.backward_path_length; - const double forward_path_length = common_parameter.forward_path_length; - const double lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const double lane_changing_duration = parameter.lane_changing_duration; - const double minimum_lane_change_length = common_parameter.minimum_lane_change_length; - const double minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; - const double maximum_deceleration = parameter.maximum_deceleration; - const int lane_change_sampling_num = parameter.lane_change_sampling_num; + const auto & backward_path_length = common_parameter.backward_path_length; + const auto & forward_path_length = common_parameter.forward_path_length; + const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; + const auto & lane_changing_duration = parameter.lane_changing_duration; + const auto & minimum_lane_change_prepare_distance = + parameter.minimum_lane_change_prepare_distance; + const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; + const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; + const auto & maximum_deceleration = parameter.maximum_deceleration; + const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; // get velocity - const double v0 = util::l2Norm(twist.linear); + const double current_velocity = util::l2Norm(twist.linear); constexpr double buffer = 1.0; // buffer for min_lane_change_length const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; @@ -100,23 +113,25 @@ std::vector getLaneChangePaths( for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - PathWithLaneId reference_path{}; - const double v1 = v0 + acceleration * lane_change_prepare_duration; - const double v2 = v1 + acceleration * lane_changing_duration; + const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration); + const double lane_changing_velocity = getExpectedVelocityWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration); // skip if velocity becomes less than zero before finishing lane change - if (v2 < 0.0) { + if (lane_changing_velocity < 0.0) { continue; } // get path on original lanes - const double straight_distance = v0 * lane_change_prepare_duration + - 0.5 * acceleration * std::pow(lane_change_prepare_duration, 2); - double lane_change_distance = - v1 * lane_changing_duration + 0.5 * acceleration * std::pow(lane_changing_duration, 2); - lane_change_distance = std::max(lane_change_distance, minimum_lane_change_length); - - if (straight_distance < target_distance) { + const double prepare_distance = getDistanceWhenDecelerate( + current_velocity, acceleration, lane_change_prepare_duration, + minimum_lane_change_prepare_distance); + const double lane_changing_distance = getDistanceWhenDecelerate( + lane_change_prepare_velocity, acceleration, lane_changing_duration, + minimum_lane_change_length + buffer); + + if (prepare_distance < target_distance) { continue; } @@ -124,21 +139,21 @@ std::vector getLaneChangePaths( { const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + straight_distance; + const double s_end = arc_position.length + prepare_distance; reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); } reference_path1.points.back().point.longitudinal_velocity_mps = std::min( reference_path1.points.back().point.longitudinal_velocity_mps, static_cast( - std::max(straight_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); + std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); PathWithLaneId reference_path2{}; { const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose); const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); - double s_start = arc_position.length + straight_distance + lane_change_distance; + double s_start = arc_position.length + prepare_distance + lane_changing_distance; s_start = std::min(s_start, lane_length - num * minimum_lane_change_length); double s_end = s_start + forward_path_length; s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer)); @@ -150,7 +165,7 @@ std::vector getLaneChangePaths( point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); } if (reference_path1.points.empty() || reference_path2.points.empty()) { @@ -162,8 +177,8 @@ std::vector getLaneChangePaths( LaneChangePath candidate_path; candidate_path.acceleration = acceleration; - candidate_path.preparation_length = straight_distance; - candidate_path.lane_change_length = lane_change_distance; + candidate_path.preparation_length = prepare_distance; + candidate_path.lane_change_length = lane_changing_distance; PathWithLaneId target_lane_reference_path; { @@ -171,7 +186,7 @@ std::vector getLaneChangePaths( lanelet::utils::getArcCoordinates( target_lanelets, reference_path1.points.back().point.pose); double s_start = lane_change_start_arc_position.length; - double s_end = s_start + straight_distance + lane_change_distance + forward_path_length; + double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); } @@ -221,8 +236,8 @@ std::vector getLaneChangePaths( } point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + static_cast(std::max( + lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); const auto nearest_idx = motion_utils::findNearestIndex(reference_path2.points, point.point.pose); point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; @@ -272,18 +287,17 @@ bool selectSafePath( const std::vector & 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 double vehicle_width, + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path) { for (const auto & path : paths) { if (isLaneChangePathSafe( path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_twist, - vehicle_width, ros_parameters, true, path.acceleration)) { + common_parameters, ros_parameters, true, path.acceleration)) { *selected_path = path; return true; } } - // set first path for force lane change if no valid path found if (!paths.empty()) { *selected_path = paths.front(); @@ -292,7 +306,6 @@ bool selectSafePath( return false; } - bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, @@ -330,92 +343,62 @@ 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 Twist & current_twist, const double vehicle_width, - const LaneChangeParameters & ros_parameters, const bool use_buffer, const double acceleration) + const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const bool use_buffer, + const double acceleration) { - if (path.points.empty()) { - return false; - } - if (target_lanes.empty() || current_lanes.empty()) { - return false; - } if (dynamic_objects == nullptr) { return true; } - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - constexpr double check_distance = 100.0; - // parameters - const double time_resolution = ros_parameters.prediction_time_resolution; - const double min_thresh = ros_parameters.min_stop_distance; - const double stop_time = ros_parameters.stop_time; - - double buffer; - double lateral_buffer; - if (use_buffer) { - buffer = ros_parameters.hysteresis_buffer_distance; - lateral_buffer = 0.5; - } else { - buffer = 0.0; - lateral_buffer = 0.0; - } - double current_lane_check_start_time = 0.0; - const double current_lane_check_end_time = - ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; - double target_lane_check_start_time = 0.0; - const double target_lane_check_end_time = - ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; - if (!ros_parameters.enable_collision_check_at_prepare_phase) { - current_lane_check_start_time = ros_parameters.lane_change_prepare_duration; - target_lane_check_start_time = ros_parameters.lane_change_prepare_duration; + if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) { + return false; } + const double time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; + const double current_lane_check_start_time = + (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration + : 0.0; + const double current_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; + double target_lane_check_start_time = + (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration + : 0.0; + const double target_lane_check_end_time = lane_change_prepare_duration + lane_changing_duration; + constexpr double ego_predicted_path_min_speed{1.0}; + const auto vehicle_predicted_path = util::convertToPredictedPath( + path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration, + ego_predicted_path_min_speed); + + const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = util::filterObjectsByLanelets(*dynamic_objects, target_lanes); // find objects in current lane + constexpr double check_distance = 100.0; const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); + + const double lateral_buffer = (use_buffer) ? 0.5 : 0.0; + const auto & vehicle_width = common_parameters.vehicle_width; + const auto & vehicle_length = common_parameters.vehicle_length; const auto current_lane_object_indices = util::filterObjectsByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); - const auto & vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration); - - // Collision check for objects in current lane for (const auto & i : current_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } + const auto predicted_paths = + util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { - double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, current_lane_check_start_time, - current_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInLaneletCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + current_lane_check_start_time, current_lane_check_end_time, time_resolution, obj, + obj_path, common_parameters)) { return false; } } @@ -424,23 +407,8 @@ bool isLaneChangePathSafe( // Collision check for objects in lane change target lane for (const auto & i : target_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } - bool is_object_in_target = false; - if (ros_parameters.use_predicted_path_outside_lanelet) { + if (lane_change_parameters.use_predicted_path_outside_lanelet) { is_object_in_target = true; } else { for (const auto & llt : target_lanes) { @@ -450,53 +418,27 @@ bool isLaneChangePathSafe( } } + const auto predicted_paths = + util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); + if (is_object_in_target) { for (const auto & obj_path : predicted_paths) { - const double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, target_lane_check_start_time, - target_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInLaneletCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, + obj_path, common_parameters)) { return false; } } } else { - const double distance = util::getDistanceBetweenPredictedPathAndObject( - obj, vehicle_predicted_path, target_lane_check_start_time, target_lane_check_end_time, - time_resolution); - double thresh = min_thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = std::max(thresh, util::l2Norm(current_twist.linear) * stop_time); - } - thresh += buffer; - if (distance < thresh) { + if (!util::isSafeInFreeSpaceCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_length, vehicle_width, + target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, + common_parameters)) { return false; } } } - return true; } - -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) -{ - tf2::Transform tf_map2ego{}; - tf2::Transform tf_map2obj{}; - Pose obj_from_ego{}; - tf2::fromMsg(ego_pose, tf_map2ego); - tf2::fromMsg(obj_pose, tf_map2obj); - tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); - - return obj_from_ego.position.x > 0; -} - -} // namespace lane_change_utils -} // namespace behavior_path_planner +} // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 87a4f31f0623f..71150b3091cc2 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -18,6 +18,9 @@ #include #include +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" + #include #include #include @@ -279,6 +282,7 @@ namespace behavior_path_planner::util using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; +using tf2::fromMsg; using tier4_autoware_utils::Point2d; std::vector convertToPointArray(const PathWithLaneId & path) @@ -353,7 +357,7 @@ PoseArray convertToGeometryPoseArray(const PathWithLaneId & path) PredictedPath convertToPredictedPath( const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double duration, const double resolution, const double acceleration) + const double duration, const double resolution, const double acceleration, double min_speed) { PredictedPath predicted_path{}; predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); @@ -368,7 +372,6 @@ PredictedPath convertToPredictedPath( auto clock{rclcpp::Clock{RCL_ROS_TIME}}; rclcpp::Time start_time = clock.now(); double vehicle_speed = std::abs(vehicle_twist.linear.x); - constexpr double min_speed = 1.0; if (vehicle_speed < min_speed) { vehicle_speed = min_speed; RCLCPP_DEBUG_STREAM_THROTTLE( @@ -723,73 +726,23 @@ std::vector filterObjectsByLanelets( bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon) { - const double obj_x = object.kinematics.initial_pose_with_covariance.pose.position.x; - const double obj_y = object.kinematics.initial_pose_with_covariance.pose.position.y; if (object.shape.type == Shape::BOUNDING_BOX) { - const double len_x = object.shape.dimensions.x; - const double len_y = object.shape.dimensions.y; - - tf2::Transform tf_map2obj; - tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_map2obj); - - // set vertices at map coordinate - tf2::Vector3 p1_map, p2_map, p3_map, p4_map; - - p1_map.setX(len_x / 2); - p1_map.setY(len_y / 2); - p1_map.setZ(0.0); - p1_map.setW(1.0); - - p2_map.setX(-len_x / 2); - p2_map.setY(len_y / 2); - p2_map.setZ(0.0); - p2_map.setW(1.0); - - p3_map.setX(-len_x / 2); - p3_map.setY(-len_y / 2); - p3_map.setZ(0.0); - p3_map.setW(1.0); - - p4_map.setX(len_x / 2); - p4_map.setY(-len_y / 2); - p4_map.setZ(0.0); - p4_map.setW(1.0); - - // transform vertices from map coordinate to object coordinate - tf2::Vector3 p1_obj, p2_obj, p3_obj, p4_obj; - - p1_obj = tf_map2obj * p1_map; - p2_obj = tf_map2obj * p2_map; - p3_obj = tf_map2obj * p3_map; - p4_obj = tf_map2obj * p4_map; - - object_polygon->outer().emplace_back(p1_obj.x(), p1_obj.y()); - object_polygon->outer().emplace_back(p2_obj.x(), p2_obj.y()); - object_polygon->outer().emplace_back(p3_obj.x(), p3_obj.y()); - object_polygon->outer().emplace_back(p4_obj.x(), p4_obj.y()); + const double & len_x = object.shape.dimensions.x; + const double & len_y = object.shape.dimensions.y; + *object_polygon = convertBoundingBoxObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, len_x, len_y); } else if (object.shape.type == Shape::CYLINDER) { - const size_t N = 20; - const double r = object.shape.dimensions.x / 2; - for (size_t i = 0; i < N; ++i) { - object_polygon->outer().emplace_back( - obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); - } + *object_polygon = convertCylindricalObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape); } else if (object.shape.type == Shape::POLYGON) { - tf2::Transform tf_map2obj; - tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_map2obj); - const auto obj_points = object.shape.footprint.points; - for (const auto & obj_point : obj_points) { - tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); - tf2::Vector3 tf_obj = tf_map2obj * obj; - object_polygon->outer().emplace_back(tf_obj.x(), tf_obj.y()); - } + *object_polygon = convertPolygonObjectToGeometryPolygon( + object.kinematics.initial_pose_with_covariance.pose, object.shape); } else { RCLCPP_WARN( rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "Object shape unknown!"); return false; } - object_polygon->outer().push_back(object_polygon->outer().front()); return true; } @@ -1996,4 +1949,397 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & length, const double & width) +{ + Polygon2d object_polygon; + + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + + // set vertices at map coordinate + tf2::Vector3 p1_map; + p1_map.setX(length / 2); + p1_map.setY(width / 2); + p1_map.setZ(0.0); + p1_map.setW(1.0); + + tf2::Vector3 p2_map; + p2_map.setX(-length / 2); + p2_map.setY(width / 2); + p2_map.setZ(0.0); + p2_map.setW(1.0); + + tf2::Vector3 p3_map; + p3_map.setX(-length / 2); + p3_map.setY(-width / 2); + p3_map.setZ(0.0); + p3_map.setW(1.0); + + tf2::Vector3 p4_map; + p4_map.setX(length / 2); + p4_map.setY(-width / 2); + p4_map.setZ(0.0); + p4_map.setW(1.0); + + // transform vertices from map coordinate to object coordinate + tf2::Vector3 p1_obj = tf_map2obj * p1_map; + tf2::Vector3 p2_obj = tf_map2obj * p2_map; + tf2::Vector3 p3_obj = tf_map2obj * p3_map; + tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int N = 20; + const double r = obj_shape.dimensions.x / 2; + for (int i = 0; i < N; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape) +{ + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + + return object_polygon; +} + +template +ProjectedDistancePoint pointToSegment( + const Point2d & reference_point, const Point2d & point_from_ego, + const Point2d & point_from_object) +{ + auto copied_point_from_object = point_from_object; + auto copied_point_from_reference = reference_point; + bg::subtract_point(copied_point_from_object, point_from_ego); + bg::subtract_point(copied_point_from_reference, point_from_ego); + + const auto c1 = bg::dot_product(copied_point_from_reference, copied_point_from_object); + if (!(c1 > 0)) { + return {point_from_ego, Pythagoras::apply(reference_point, point_from_ego)}; + } + + const auto c2 = bg::dot_product(copied_point_from_object, copied_point_from_object); + if (!(c2 > c1)) { + return {point_from_object, Pythagoras::apply(reference_point, point_from_object)}; + } + + Point2d projected = point_from_ego; + bg::multiply_value(copied_point_from_object, c1 / c2); + bg::add_point(projected, copied_point_from_object); + + return {projected, Pythagoras::apply(reference_point, projected)}; +} + +void getProjectedDistancePointFromPolygons( + const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, + Pose & point_on_object) +{ + ProjectedDistancePoint nearest; + std::unique_ptr current_point; + + bool points_in_ego{false}; + + { + const auto segments = + boost::make_iterator_range(bg::segments_begin(ego_polygon), bg::segments_end(ego_polygon)); + const auto points = + boost::make_iterator_range(bg::points_begin(object_polygon), bg::points_end(object_polygon)); + + for (auto && segment : segments) { + for (auto && point : points) { + const auto projected = pointToSegment(point, *segment.first, *segment.second); + if (!current_point || projected.distance < nearest.distance) { + current_point = std::make_unique(point); + nearest = projected; + points_in_ego = false; + } + } + } + } + + { + const auto segments = boost::make_iterator_range( + bg::segments_begin(object_polygon), bg::segments_end(object_polygon)); + const auto points = + boost::make_iterator_range(bg::points_begin(ego_polygon), bg::points_end(ego_polygon)); + + for (auto && segment : segments) { + for (auto && point : points) { + const auto projected = pointToSegment(point, *segment.first, *segment.second); + if (!current_point || projected.distance < nearest.distance) { + current_point = std::make_unique(point); + nearest = projected; + points_in_ego = true; + } + } + } + } + + if (!points_in_ego) { + point_on_object.position.x = current_point->x(); + point_on_object.position.y = current_point->y(); + point_on_ego.position.x = nearest.projected_point.x(); + point_on_ego.position.y = nearest.projected_point.y(); + } else { + point_on_ego.position.x = current_point->x(); + point_on_ego.position.y = current_point->y(); + point_on_object.position.x = nearest.projected_point.x(); + point_on_object.position.y = nearest.projected_point.y(); + } +} + +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) +{ + if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { + return false; + } + expected_pose.orientation = current_pose.orientation; + ego_polygon = util::convertBoundingBoxObjectToGeometryPolygon(expected_pose, length, width); + + return true; +} + +bool getObjectExpectedPoseAndConvertToPolygon( + const PredictedPath & pred_path, const PredictedObject & object, Pose & expected_pose, + Polygon2d & obj_polygon, const double & check_current_time) +{ + if (!util::lerpByTimeStamp(pred_path, check_current_time, &expected_pose)) { + return false; + } + expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + + if (!util::calcObjectPolygon(object, &obj_polygon)) { + return false; + } + + return true; +} + +std::vector getPredictedPathFromObj( + const PredictedObject & obj, const bool & is_use_all_predicted_path) +{ + std::vector predicted_path_vec; + if (is_use_all_predicted_path) { + std::copy( + obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), + predicted_path_vec.begin()); + } else { + const auto max_confidence_path = std::max_element( + obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), + [](const auto & path1, const auto & path2) { return path1.confidence > path2.confidence; }); + if (max_confidence_path != obj.kinematics.predicted_paths.end()) { + predicted_path_vec.push_back(*max_confidence_path); + } + } + return predicted_path_vec; +} + +Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object) +{ + tf2::Transform tf_map_desired_to_global{}; + tf2::Transform tf_map_target_to_global{}; + + tf2::fromMsg(desired_object, tf_map_desired_to_global); + tf2::fromMsg(target_object, tf_map_target_to_global); + + Pose desired_obj_pose_projected_to_target{}; + tf2::toMsg( + tf_map_desired_to_global.inverse() * tf_map_target_to_global, + desired_obj_pose_projected_to_target); + + return desired_obj_pose_projected_to_target; +} + +bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) +{ + const auto obj_from_ego = projectCurrentPoseToTarget(ego_pose, obj_pose); + return obj_from_ego.position.x > 0; +} + +bool isObjectFront(const Pose & projected_ego_pose) { return projected_ego_pose.position.x > 0; } + +double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel) +{ + const auto deceleration = (vehicle_accel < -1e-3) ? vehicle_accel : -1.0; + return -std::pow(vehicle_velocity, 2) / (2.0 * deceleration); +} + +double frontVehicleStopDistance( + const double & front_vehicle_velocity, const double & front_vehicle_accel, + const double & distance_to_collision) +{ + return stoppingDistance(front_vehicle_velocity, front_vehicle_accel) + distance_to_collision; +} + +double rearVehicleStopDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time, const double & rear_vehicle_safety_time_margin) +{ + return rear_vehicle_velocity * rear_vehicle_reaction_time + + stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel) + + rear_vehicle_velocity * rear_vehicle_safety_time_margin; +} + +bool isLongitudinalDistanceEnough( + const double & rear_vehicle_stop_threshold, const double & front_vehicle_stop_threshold) +{ + return rear_vehicle_stop_threshold < front_vehicle_stop_threshold; +} + +bool hasEnoughDistance( + const Pose & expected_ego_pose, const Twist & ego_current_twist, + const Pose & expected_object_pose, const Twist & object_current_twist, + const BehaviorPathPlannerParameters & param) +{ + const auto front_vehicle_pose = + projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); + + if (isLateralDistanceEnough( + front_vehicle_pose.position.y, param.lateral_distance_max_threshold)) { + return true; + } + + const auto is_obj_in_front = isObjectFront(front_vehicle_pose); + + const auto front_vehicle_velocity = + (is_obj_in_front) ? object_current_twist.linear : ego_current_twist.linear; + + const auto rear_vehicle_velocity = + (is_obj_in_front) ? ego_current_twist.linear : object_current_twist.linear; + + const auto front_vehicle_accel = param.expected_front_deceleration; + const auto rear_vehicle_accel = param.expected_rear_deceleration; + + const auto front_vehicle_stop_threshold = frontVehicleStopDistance( + util::l2Norm(front_vehicle_velocity), front_vehicle_accel, + std::fabs(front_vehicle_pose.position.x)); + + const auto rear_vehicle_stop_threshold = std::max( + rearVehicleStopDistance( + util::l2Norm(rear_vehicle_velocity), rear_vehicle_accel, param.rear_vehicle_reaction_time, + param.rear_vehicle_safety_time_margin), + param.longitudinal_distance_min_threshold); + + return isLongitudinalDistanceEnough(rear_vehicle_stop_threshold, front_vehicle_stop_threshold); +} + +bool isLateralDistanceEnough( + const double & relative_lateral_distance, const double & lateral_distance_threshold) +{ + return std::fabs(relative_lateral_distance) > lateral_distance_threshold; +} + +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 double & check_time_resolution, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters) +{ + for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + Pose expected_obj_pose; + tier4_autoware_utils::Polygon2d obj_polygon; + if (!util::getObjectExpectedPoseAndConvertToPolygon( + target_object_path, target_object, expected_obj_pose, obj_polygon, t)) { + continue; + } + + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, + ego_vehicle_length, ego_vehicle_width)) { + continue; + } + + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + return false; + } + + util::getProjectedDistancePointFromPolygons( + ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + if (!util::hasEnoughDistance( + expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, + common_parameters)) { + return false; + } + } + return true; +} + +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 double & check_time_resolution, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters) +{ + tier4_autoware_utils::Polygon2d obj_polygon; + if (!util::calcObjectPolygon(target_object, &obj_polygon)) { + return false; + } + for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!util::getEgoExpectedPoseAndConvertToPolygon( + ego_current_pose, ego_predicted_path, expected_ego_pose, ego_polygon, t, + ego_vehicle_length, ego_vehicle_width)) { + continue; + } + + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + return false; + } + + auto expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; + util::getProjectedDistancePointFromPolygons( + ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); + + const auto object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + if (!util::hasEnoughDistance( + expected_ego_pose, ego_current_twist, + target_object.kinematics.initial_pose_with_covariance.pose, object_twist, + common_parameters)) { + return false; + } + } + return true; +} } // namespace behavior_path_planner::util diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp new file mode 100644 index 0000000000000..fb5557170b39a --- /dev/null +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -0,0 +1,37 @@ +// 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. +#include "behavior_path_planner/scene_module/lane_change/util.hpp" + +#include +#include + +TEST(BehaviorPathPlanningLaneChangeUtilsTest, testStoppingDistance) +{ + const auto vehicle_velocity = 8.333; + + const auto negative_accel = -1.5; + const auto distance_when_negative = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, negative_accel); + ASSERT_NEAR(distance_when_negative, 23.1463, 1e-3); + + const auto positive_accel = 1.5; + const auto distance_when_positive = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, positive_accel); + ASSERT_NEAR(distance_when_positive, 34.7194, 1e-3); + + const auto zero_accel = 0.0; + const auto distance_when_zero = + behavior_path_planner::lane_change_utils::stoppingDistance(vehicle_velocity, zero_accel); + ASSERT_NEAR(distance_when_zero, 34.7194, 1e-3); +} From 020005e625a32aef240a907e9d93b0b035cd2395 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 22 Aug 2022 11:34:48 +0900 Subject: [PATCH 2/3] refactor and copy predicted if not empty Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/src/utilities.cpp | 48 +++++++------------ 1 file changed, 16 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 71150b3091cc2..c47079725439f 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2071,29 +2071,13 @@ void getProjectedDistancePointFromPolygons( bool points_in_ego{false}; - { - const auto segments = - boost::make_iterator_range(bg::segments_begin(ego_polygon), bg::segments_end(ego_polygon)); - const auto points = - boost::make_iterator_range(bg::points_begin(object_polygon), bg::points_end(object_polygon)); - - for (auto && segment : segments) { - for (auto && point : points) { - const auto projected = pointToSegment(point, *segment.first, *segment.second); - if (!current_point || projected.distance < nearest.distance) { - current_point = std::make_unique(point); - nearest = projected; - points_in_ego = false; - } - } - } - } - - { + const auto findPoints = [&nearest, ¤t_point, &points_in_ego]( + const Polygon2d & polygon_for_segment, + const Polygon2d & polygon_for_points, const bool & ego_is_points) { const auto segments = boost::make_iterator_range( - bg::segments_begin(object_polygon), bg::segments_end(object_polygon)); - const auto points = - boost::make_iterator_range(bg::points_begin(ego_polygon), bg::points_end(ego_polygon)); + bg::segments_begin(polygon_for_segment), bg::segments_end(polygon_for_segment)); + const auto points = boost::make_iterator_range( + bg::points_begin(polygon_for_points), bg::points_end(polygon_for_points)); for (auto && segment : segments) { for (auto && point : points) { @@ -2101,11 +2085,14 @@ void getProjectedDistancePointFromPolygons( if (!current_point || projected.distance < nearest.distance) { current_point = std::make_unique(point); nearest = projected; - points_in_ego = true; + points_in_ego = ego_is_points; } } } - } + }; + + std::invoke(findPoints, ego_polygon, object_polygon, false); + std::invoke(findPoints, object_polygon, ego_polygon, true); if (!points_in_ego) { point_on_object.position.x = current_point->x(); @@ -2143,11 +2130,7 @@ bool getObjectExpectedPoseAndConvertToPolygon( } expected_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; - if (!util::calcObjectPolygon(object, &obj_polygon)) { - return false; - } - - return true; + return util::calcObjectPolygon(object, &obj_polygon); } std::vector getPredictedPathFromObj( @@ -2155,9 +2138,10 @@ std::vector getPredictedPathFromObj( { std::vector predicted_path_vec; if (is_use_all_predicted_path) { - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_path_vec.begin()); + std::copy_if( + obj.kinematics.predicted_paths.cbegin(), obj.kinematics.predicted_paths.cend(), + std::back_inserter(predicted_path_vec), + [](const PredictedPath & path) { return !path.path.empty(); }); } else { const auto max_confidence_path = std::max_element( obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), From d50abecc013f76a3cd2e0f944b829b30bcb04eff Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Mon, 22 Aug 2022 17:04:42 +0900 Subject: [PATCH 3/3] Include abort parameters and reorganize parameters declaration Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 26 +++++++++++-------- .../lane_change/lane_change_module.hpp | 1 - .../src/behavior_path_planner_node.cpp | 9 +++---- 3 files changed, 19 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 0cd8fa4f7d374..5ef1d10980184 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,15 +1,19 @@ /**: ros__parameters: lane_change: - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_velocity: 5.6 - prediction_time_resolution: 0.5 - maximum_deceleration: 1.0 + lane_change_prepare_duration: 4.0 # [s] + lane_changing_duration: 8.0 # [s] + minimum_lane_change_prepare_distance: 4.0 # [m] + lane_change_finish_judge_buffer: 3.0 # [m] + minimum_lane_change_velocity: 5.6 # [m/s] + prediction_time_resolution: 0.5 # [s] + maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 - enable_abort_lane_change: false - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - lane_change_search_distance: 30.0 + abort_lane_change_velocity_thresh: 0.5 + abort_lane_change_angle_thresh: 10.0 # [deg] + abort_lane_change_distance_thresh: 0.3 # [m] + enable_abort_lane_change: true + enable_collision_check_at_prepare_phase: true + use_predicted_path_outside_lanelet: true + use_all_predicted_path: true + publish_debug_marker: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 7d4c7c4d8174c..b649e38cf2833 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -49,7 +49,6 @@ struct LaneChangeParameters bool enable_collision_check_at_prepare_phase; bool use_predicted_path_outside_lanelet; bool use_all_predicted_path; - double lane_change_search_distance; }; struct LaneChangeStatus diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e04fff4583ebc..e0a423878e3c3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -322,15 +322,14 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); p.maximum_deceleration = dp("maximum_deceleration", 1.0); p.lane_change_sampling_num = dp("lane_change_sampling_num", 10); - p.enable_abort_lane_change = dp("enable_abort_lane_change", true); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); - p.use_all_predicted_path = dp("use_all_predicted_path", false); p.abort_lane_change_velocity_thresh = dp("abort_lane_change_velocity_thresh", 0.5); p.abort_lane_change_angle_thresh = dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0)); p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3); - p.lane_change_search_distance = dp("lane_change_search_distance", 30.0); + p.enable_abort_lane_change = dp("enable_abort_lane_change", true); + p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); + p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); + p.use_all_predicted_path = dp("use_all_predicted_path", false); // validation of parameters if (p.lane_change_sampling_num < 1) {