diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 9c94752c1138f..42dfecfc69143 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -49,6 +49,13 @@ if(BUILD_TESTING) target_link_libraries(test_${PROJECT_NAME}_utilities behavior_path_planner_node ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_lane_change_utils + test/test_lane_change_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME}_lane_change_utils + behavior_path_planner_node + ) endif() ament_auto_package( 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 50934a74542ea..810fa2561e14d 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 @@ -12,9 +12,15 @@ 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 + safety_time_margin_for_control: 2.0 + rear_vehicle_reaction_time: 1.0 + lateral_distance_threshold: 5.0 + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 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 41cbcbbbc1d61..5fafac1264ce6 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 @@ -61,6 +61,11 @@ struct LaneChangeParameters bool use_all_predicted_path; bool enable_blocked_by_obstacle; double lane_change_search_distance; + double safety_time_margin_for_control; + double rear_vehicle_reaction_time; + double lateral_distance_threshold; + double expected_front_deceleration; + double expected_rear_deceleration; }; struct LaneChangeStatus @@ -172,7 +177,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..cd4f99f9a0fa2 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,10 +31,9 @@ #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; @@ -59,14 +58,14 @@ 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 double & vehicle_width, const double & vehicle_length, const behavior_path_planner::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 Twist & current_twist, const double & vehicle_width, const double & vehicle_length, const behavior_path_planner::LaneChangeParameters & ros_parameters, const bool use_buffer = true, const double acceleration = 0.0); bool hasEnoughDistance( @@ -74,8 +73,36 @@ bool hasEnoughDistance( const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); + +std::vector getPredictedPathFromObj( + const PredictedObject & obj, const bool & is_use_all_predicted_path); + +Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object); + bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); -} // namespace lane_change_utils -} // namespace behavior_path_planner + +double stoppingDistance(const double & vehicle_velocity, const double & vehicle_accel); + +double stoppingDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time); + +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 & safety_time_margin_for_control); + +bool isUnderThresholdDistanceSafe( + 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 LaneChangeParameters & param); + +} // 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 3c5e5a4fc4676..eaa7daf8d3797 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -125,6 +125,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; @@ -137,7 +139,6 @@ 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; struct FrenetCoordinate3d @@ -346,6 +347,13 @@ bool checkLaneIsInIntersection( // object label std::uint8_t getHighestProbLabel(const std::vector & classification); +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); } // namespace util } // namespace behavior_path_planner 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 7486b59b15191..c09a7a68fd509 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -334,6 +334,11 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() 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); + p.safety_time_margin_for_control = dp("safety_time_margin_for_control", 2.0); + p.rear_vehicle_reaction_time = dp("rear_vehicle_reaction_time", 1.0); + p.lateral_distance_threshold = dp("lateral_distance_threshold", 5.0); + p.expected_front_deceleration = dp("expected_front_deceleration", -1.0); + p.expected_rear_deceleration = dp("expected_rear_deceleration", -1.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 442437db94f48..7675bd1a9ce95 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 @@ -115,7 +115,7 @@ bool LaneChangeModule::isExecutionReady() const std::tie(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() @@ -385,7 +385,8 @@ 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.vehicle_width, common_parameters.vehicle_length, parameters_, + &safe_path); return std::make_pair(true, found_safe_path); } @@ -411,54 +412,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; @@ -501,7 +454,8 @@ 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.vehicle_width, common_parameters.vehicle_length, 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 c32c636c453f1..867a2e433adc5 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 @@ -30,10 +30,10 @@ #include #include -namespace behavior_path_planner -{ -namespace lane_change_utils +namespace behavior_path_planner::lane_change_utils { +using tier4_autoware_utils::Polygon2d; + PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) { PathWithLaneId path; @@ -272,24 +272,18 @@ 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 double & vehicle_width, const double & vehicle_length, 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)) { + vehicle_width, vehicle_length, 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(); - return false; - } - return false; } @@ -330,93 +324,107 @@ 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 Twist & current_twist, const double & vehicle_width, const double & vehicle_length, const LaneChangeParameters & ros_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 = ros_parameters.prediction_time_resolution; + const auto & lane_change_prepare_duration = ros_parameters.lane_change_prepare_duration; + const auto & lane_changing_duration = ros_parameters.lane_changing_duration; + const double current_lane_check_start_time = + (!ros_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 = + (!ros_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; + const auto vehicle_predicted_path = util::convertToPredictedPath( + path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration); + + const auto getEgoExpectedPoseAndConvertToPolygon = + []( + 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; + } + ego_polygon = util::convertBoundingBoxObjectToGeometryPolygon(expected_pose, length, width); + + return true; + }; + + const auto 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; + } + + if (!util::calcObjectPolygon(object, &obj_polygon)) { + return false; + } + + return true; + }; + + 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 current_lane_object_indices = util::filterObjectsByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, vehicle_width / 2 + lateral_buffer); + constexpr double safety_buffer = 0.5; + const auto vehicle_width_with_buffer = vehicle_width + safety_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 = + getPredictedPathFromObj(obj, ros_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) { - return false; + for (double t = current_lane_check_start_time; t < current_lane_check_end_time; + t += time_resolution) { + Pose expected_obj_pose; + tier4_autoware_utils::Polygon2d obj_polygon; + if (!getObjectExpectedPoseAndConvertToPolygon( + obj_path, obj, expected_obj_pose, obj_polygon, t)) { + continue; + } + + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!getEgoExpectedPoseAndConvertToPolygon( + vehicle_predicted_path, expected_ego_pose, ego_polygon, t, vehicle_length, + vehicle_width_with_buffer)) { + continue; + } + + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + return false; + } + + const auto & object_twist = obj.kinematics.initial_twist_with_covariance.twist; + if (!hasEnoughDistance( + expected_ego_pose, current_twist, expected_obj_pose, object_twist, ros_parameters)) { + return false; + } } } } @@ -424,21 +432,6 @@ 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) { is_object_in_target = true; @@ -450,53 +443,176 @@ bool isLaneChangePathSafe( } } + const auto predicted_paths = + getPredictedPathFromObj(obj, ros_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) { - return false; + for (double t = target_lane_check_start_time; t < target_lane_check_end_time; + t += time_resolution) { + Pose expected_obj_pose; + tier4_autoware_utils::Polygon2d obj_polygon; + if (!getObjectExpectedPoseAndConvertToPolygon( + obj_path, obj, expected_obj_pose, obj_polygon, t)) { + continue; + } + + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!getEgoExpectedPoseAndConvertToPolygon( + vehicle_predicted_path, expected_ego_pose, ego_polygon, t, vehicle_length, + vehicle_width_with_buffer)) { + continue; + } + + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + return false; + } + + const auto & object_twist = obj.kinematics.initial_twist_with_covariance.twist; + if (!hasEnoughDistance( + expected_ego_pose, current_twist, expected_obj_pose, object_twist, + ros_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) { + tier4_autoware_utils::Polygon2d obj_polygon; + if (!util::calcObjectPolygon(obj, &obj_polygon)) { return false; } + for (double t = target_lane_check_start_time; t < target_lane_check_end_time; + t += time_resolution) { + Pose expected_ego_pose; + tier4_autoware_utils::Polygon2d ego_polygon; + if (!getEgoExpectedPoseAndConvertToPolygon( + vehicle_predicted_path, expected_ego_pose, ego_polygon, t, vehicle_length, + vehicle_width_with_buffer)) { + continue; + } + + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { + return false; + } + const auto object_twist = obj.kinematics.initial_twist_with_covariance.twist; + if (!hasEnoughDistance( + expected_ego_pose, current_twist, obj.kinematics.initial_pose_with_covariance.pose, + object_twist, ros_parameters)) { + return false; + } + } } } - return true; } -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) +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_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); + 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; } -} // namespace lane_change_utils -} // namespace behavior_path_planner +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 stoppingDistance( + const double & rear_vehicle_velocity, const double & rear_vehicle_accel, + const double & rear_vehicle_reaction_time) +{ + return rear_vehicle_velocity * rear_vehicle_reaction_time + + stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel); +} +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 & safety_time_margin_for_control) +{ + return stoppingDistance(rear_vehicle_velocity, rear_vehicle_accel, rear_vehicle_reaction_time) + + rear_vehicle_velocity * safety_time_margin_for_control; +} + +bool isUnderThresholdDistanceSafe( + 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 LaneChangeParameters & param) +{ + const auto front_vehicle_pose = + projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); + + if (param.lateral_distance_threshold < std::fabs(front_vehicle_pose.position.y)) { + return true; + } + + const auto velocity_based_on_ego_position = [&](const bool & object_is_in_front) noexcept { + return object_is_in_front ? util::l2Norm(object_current_twist.linear) + : util::l2Norm(ego_current_twist.linear); + }; + + const auto is_obj_in_front = isObjectFront(expected_ego_pose, expected_object_pose); + + const auto front_vehicle_velocity = velocity_based_on_ego_position(is_obj_in_front); + const auto rear_vehicle_velocity = velocity_based_on_ego_position(!is_obj_in_front); + 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( + front_vehicle_velocity, front_vehicle_accel, std::fabs(front_vehicle_pose.position.x)); + const auto rear_vehicle_stop_threshold = rearVehicleStopDistance( + rear_vehicle_velocity, rear_vehicle_accel, param.rear_vehicle_reaction_time, + param.safety_time_margin_for_control); + + return isUnderThresholdDistanceSafe(rear_vehicle_stop_threshold, front_vehicle_stop_threshold); +} + +} // 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 74428ada3cf38..193d55974ff97 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -215,6 +215,8 @@ namespace 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) { @@ -691,73 +693,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; } @@ -1839,5 +1791,89 @@ std::uint8_t getHighestProbLabel(const std::vector & class return label; } +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; +} } // namespace util } // namespace behavior_path_planner 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); +}