diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5e2e4673166da..e96b254900869 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -10,6 +10,7 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false + enable_safety_check: false # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] @@ -23,11 +24,20 @@ object_check_shiftable_ratio: 0.6 # [-] object_check_min_road_shoulder_width: 0.5 # [m] + # For safety check + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + # For lateral margin object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] + # For longitudinal margin + longitudinal_collision_margin_buffer: 0.0 # [m] + prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_avoidance_distance: 10.0 # [m] @@ -74,3 +84,9 @@ # ---------- advanced parameters ---------- avoidance_execution_lateral_threshold: 0.499 + + target_velocity_matrix: + col_size: 4 + matrix: + [2.78, 5.56, 11.1, 13.9, # velocity [m/s] + 0.20, 0.90, 1.10, 1.20] # margin [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 68c3efd1b75a1..8e812ae915d63 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -238,7 +238,9 @@ class AvoidanceModule : public SceneModuleInterface // shift point generation: generator double getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const; - AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const; + + AvoidLineArray calcRawShiftLinesFromObjects( + const AvoidancePlanningData & data, DebugData & debug) const; // shift point generation: combiner AvoidLineArray combineRawShiftLinesWithUniqueCheck( @@ -307,6 +309,30 @@ class AvoidanceModule : public SceneModuleInterface mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; + double getLateralMarginFromVelocity(const double velocity) const; + + double getRSSLongitudinalDistance( + const double v_ego, const double v_obj, const bool is_front_object) const; + + ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; + + // ========= safety check ============== + + lanelet::ConstLanelets getAdjacentLane( + const PathShifter & path_shifter, const double forward_distance, + const double backward_distance) const; + + bool isSafePath( + const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; + + bool isSafePath( + const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, + DebugData & debug) const; + + bool isEnoughMargin( + const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, + MarginData & margin_data) const; + // ========= helper functions ========== double getEgoSpeed() const diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 426387c939165..90e5a2acf902d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -26,6 +26,7 @@ #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Point2d; @@ -68,6 +70,9 @@ struct AvoidanceParameters // enable update path when if detected objects on planner data is gone. bool enable_update_path_when_object_is_gone{false}; + // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver + bool enable_safety_check{false}; + // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. double threshold_distance_object_is_on_center; @@ -99,6 +104,9 @@ struct AvoidanceParameters // don't ever set this value to 0 double lateral_collision_safety_buffer{0.5}; + // margin between object back and end point of avoid shift point + double longitudinal_collision_safety_buffer{0.0}; + // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction double longitudinal_collision_margin_min_distance; @@ -107,6 +115,21 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; + // find adjacent lane vehicles + double safety_check_backward_distance; + + // minimum longitudinal margin for vehicles in adjacent lane + double safety_check_min_longitudinal_margin; + + // safety check time horizon + double safety_check_time_horizon; + + // use in RSS calculation + double safety_check_idling_time; + + // use in RSS calculation + double safety_check_accel_for_rss; + // start avoidance after this time to avoid sudden path change double prepare_time; @@ -163,6 +186,12 @@ struct AvoidanceParameters // avoidance path will be generated unless their lateral margin difference exceeds this value. double avoidance_execution_lateral_threshold; + // target velocity matrix + std::vector target_velocity_matrix; + + // matrix col size + size_t col_size; + // true by default bool avoid_car{true}; // avoidance is performed for type object car bool avoid_truck{true}; // avoidance is performed for type object truck @@ -234,6 +263,9 @@ struct ObjectData // avoidance target // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; + + // unavoidable reason + std::string reason{""}; }; using ObjectDataArray = std::vector; @@ -301,6 +333,8 @@ struct AvoidancePlanningData // new shift point AvoidLineArray unapproved_new_sl{}; + bool safe{false}; + bool avoiding_now{false}; }; @@ -328,6 +362,27 @@ struct ShiftLineData std::vector> shift_line_history; }; +/* + * Data struct for longitudinal margin + */ +struct MarginData +{ + Pose pose{}; + + bool enough_lateral_margin{true}; + + double longitudinal_distance{std::numeric_limits::max()}; + + double longitudinal_margin{std::numeric_limits::lowest()}; + + double vehicle_width; + + double base_link2front; + + double base_link2rear; +}; +using MarginDataArray = std::vector; + /* * Debug information for marker array */ @@ -356,6 +411,20 @@ struct DebugData std::vector total_shift; std::vector output_shift; + bool exist_adjacent_objects{false}; + + // future pose + PathWithLaneId path_with_planned_velocity; + + // margin + MarginDataArray margin_data_array; + + // avoidance require objects + ObjectDataArray unavoidable_objects; + + // avoidance unsafe objects + ObjectDataArray unsafe_objects; + // tmp for plot PathWithLaneId center_line; AvoidanceDebugMsgArray avoidance_debug_msg_array; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index ce04323a775dc..5e2ecf2e61211 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -101,6 +101,15 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const ObjectDataArray & objects, const bool enable_bound_clipping); + +double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); + +lanelet::ConstLanelets getTargetLanelets( + const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, + const double left_offset, const double right_offset); } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index 60e31b0a3a416..5d1ff1254eb24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -36,6 +36,7 @@ namespace marker_utils::avoidance_marker { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::ShiftLineArray; @@ -44,15 +45,22 @@ using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; +MarkerArray createEgoStatusMarkerArray( + const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); + MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, const float & b, const double & w); -MarkerArray createTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); +MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); -MarkerArray createOtherObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns); +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); + +MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); 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 9b997e218af27..42218f54c746d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include namespace behavior_path_planner::util @@ -88,7 +89,7 @@ struct ProjectedDistancePoint double distance{0.0}; }; -template > +template > ProjectedDistancePoint pointToSegment( const Point2d & reference_point, const Point2d & point_from_ego, const Point2d & point_from_object); @@ -260,13 +261,19 @@ std::vector filterObjectIndicesByLanelets( const double start_arc_length, const double end_arc_length); /** - * @brief Get index of the obstacles inside the lanelets - * @return Indices corresponding to the obstacle inside the lanelets + * @brief Separate index of the obstacles into two part based on whether the object is within + * lanelet. + * @return Indices of objects pair. first objects are in the lanelet, and second others are out of + * lanelet. */ -std::vector filterObjectIndicesByLanelets( +std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); -PredictedObjects filterObjectsByLanelets( +/** + * @brief Separate the objects into two part based on whether the object is within lanelet. + * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. + */ +std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); std::vector filterObjectsIndicesByPath( 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 c6cd47e91c50b..562d3cc7b1513 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -288,6 +288,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true); p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true); p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false); + p.enable_safety_check = dp("enable_safety_check", false); p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0); p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0); @@ -299,6 +300,13 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); + p.longitudinal_collision_safety_buffer = dp("longitudinal_collision_safety_buffer", 0.0); + + p.safety_check_min_longitudinal_margin = dp("safety_check_min_longitudinal_margin", 0.0); + p.safety_check_backward_distance = dp("safety_check_backward_distance", 0.0); + p.safety_check_time_horizon = dp("safety_check_time_horizon", 10.0); + p.safety_check_idling_time = dp("safety_check_idling_time", 1.5); + p.safety_check_accel_for_rss = dp("safety_check_accel_for_rss", 2.5); p.prepare_time = dp("prepare_time", 3.0); p.min_prepare_distance = dp("min_prepare_distance", 10.0); @@ -327,6 +335,13 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.publish_debug_marker = dp("publish_debug_marker", false); p.print_debug_info = dp("print_debug_info", false); + // velocity matrix + { + std::string ns = "avoidance.target_velocity_matrix."; + p.col_size = declare_parameter(ns + "col_size"); + p.target_velocity_matrix = declare_parameter>(ns + "matrix"); + } + p.avoid_car = dp("target_object.car", true); p.avoid_truck = dp("target_object.truck", true); p.avoid_bus = dp("target_object.bus", true); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 61963bb9f0aaa..cc0f880223e65 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -47,7 +47,12 @@ using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; +using tier4_autoware_utils::calcLongitudinalDeviation; +using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getPose; using tier4_planning_msgs::msg::AvoidanceDebugFactor; namespace @@ -220,14 +225,22 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // detection area filter // when expanding lanelets, right_offset must be minus. // This is because y axis is positive on the left. - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, parameters_->detection_area_left_expand_dist, + const auto expanded_lanelets = getTargetLanelets( + planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, parameters_->detection_area_right_expand_dist * (-1.0)); - const auto lane_filtered_objects_index = - util::filterObjectIndicesByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + const auto [object_within_target_lane, object_outside_target_lane] = + util::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + for (const auto & object : object_outside_target_lane.objects) { + ObjectData other_object; + other_object.object = object; + other_object.reason = "OutOfTargetArea"; + data.other_objects.push_back(other_object); + } DEBUG_PRINT("dynamic_objects size = %lu", planner_data_->dynamic_object->objects.size()); - DEBUG_PRINT("lane_filtered_objects size = %lu", lane_filtered_objects_index.size()); + DEBUG_PRINT("lane_filtered_objects size = %lu", object_within_target_lane.objects.size()); // for goal const auto & rh = planner_data_->route_handler; @@ -241,8 +254,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // for filtered objects ObjectDataArray target_objects; std::vector avoidance_debug_msg_array; - for (const auto & i : lane_filtered_objects_index) { - const auto & object = planner_data_->dynamic_object->objects.at(i); + for (const auto & object : object_within_target_lane.objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = @@ -252,15 +264,17 @@ void AvoidanceModule::fillAvoidanceTargetObjects( avoidance_debug_msg_array.push_back(avoidance_debug_msg); }; + ObjectData object_data; + object_data.object = object; + avoidance_debug_msg.object_id = getUuidStr(object_data); + if (!isTargetObjectType(object)) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; + data.other_objects.push_back(object_data); continue; } - ObjectData object_data; - object_data.object = object; - avoidance_debug_msg.object_id = getUuidStr(object_data); - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; @@ -279,6 +293,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( if (object_data.move_time > parameters_->threshold_time_object_is_moving) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); + object_data.reason = AvoidanceDebugFactor::MOVING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -286,11 +301,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // object is behind ego or too far. if (object_data.longitudinal < -parameters_->object_check_backward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; data.other_objects.push_back(object_data); continue; } if (object_data.longitudinal > parameters_->object_check_forward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; data.other_objects.push_back(object_data); continue; } @@ -298,6 +315,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Target object is behind the path goal -> ignore. if (object_data.longitudinal > dist_to_goal) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; data.other_objects.push_back(object_data); continue; } @@ -350,6 +368,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; if (std::abs(object_data.lateral) < parameters_->threshold_distance_object_is_on_center) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + object_data.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; data.other_objects.push_back(object_data); continue; } @@ -381,6 +400,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -432,6 +452,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( if (!is_parking_object) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + object_data.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; data.other_objects.push_back(object_data); continue; } @@ -531,7 +552,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Create raw shift points from target object. The lateral margin between the ego and the target * object varies depending on the relative speed between the ego and the target object. */ - data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data.target_objects); + data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data, debug); /** * STEP 2 @@ -557,10 +578,20 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP 5 * Generate avoidance path. */ - data.candidate_path = generateAvoidancePath(path_shifter); + auto candidate_path = generateAvoidancePath(path_shifter); + + /** + * STEP 6 + * Check avoidance path safety. For each target objects and the objects in adjacent lanes, + * check that there is a certain amount of margin in the lateral and longitudinal direction. + */ + data.safe = isSafePath(path_shifter, candidate_path, debug); + data.candidate_path = candidate_path; { + debug.output_shift = data.candidate_path.shift_length; debug.current_raw_shift = data.unapproved_raw_sl; + debug.new_shift_lines = data.unapproved_new_sl; } } @@ -707,9 +738,14 @@ double AvoidanceModule::getShiftLength( * Calculate the shift points (start/end point, shift length) from the object lateral * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here. */ -AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const +AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( + const AvoidancePlanningData & data, DebugData & debug) const { - debug_avoidance_initializer_for_shift_line_.clear(); + { + debug_avoidance_initializer_for_shift_line_.clear(); + debug.unavoidable_objects.clear(); + } + const auto prepare_distance = getNominalPrepareDistance(); // To be consistent with changes in the ego position, the current shift length is considered. @@ -724,8 +760,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr AvoidLineArray avoid_lines; std::vector avoidance_debug_msg_array; - avoidance_debug_msg_array.reserve(objects.size()); - for (auto & o : objects) { + avoidance_debug_msg_array.reserve(data.target_objects.size()); + for (auto o : data.target_objects) { AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { @@ -751,6 +787,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr } else { avoidance_debug_array_false_and_push_back( AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); + o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + debug.unavoidable_objects.push_back(o); continue; } } @@ -759,6 +797,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr const auto shift_length = getShiftLength(o, is_object_on_right, avoid_margin); if (isSameDirectionShift(is_object_on_right, shift_length)) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); + o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; + debug.unavoidable_objects.push_back(o); continue; } @@ -786,6 +826,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr // is not zero. avoidance_debug_array_false_and_push_back( AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO); + if (!data.avoiding_now) { + o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + debug.unavoidable_objects.push_back(o); + } continue; } @@ -796,6 +840,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr avoidance_debug_msg.maximum_jerk = parameters_->max_lateral_jerk; if (required_jerk > parameters_->max_lateral_jerk) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::TOO_LARGE_JERK); + if (!data.avoiding_now) { + o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + debug.unavoidable_objects.push_back(o); + } continue; } } @@ -1939,6 +1987,370 @@ void AvoidanceModule::addReturnShiftLineFromEgo( DEBUG_PRINT("Return Shift is added."); } +bool AvoidanceModule::isSafePath( + const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const +{ + const auto & p = parameters_; + + if (!p->enable_safety_check) { + return true; // if safety check is disabled, it always return safe. + } + + const auto & forward_check_distance = p->object_check_forward_distance; + const auto & backward_check_distance = p->safety_check_backward_distance; + const auto check_lanes = + getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); + + auto path_with_current_velocity = shifted_path.path; + path_with_current_velocity = util::resamplePathWithSpline(path_with_current_velocity, 0.5); + + const size_t ego_idx = findEgoIndex(path_with_current_velocity.points); + util::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); + + constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h + for (auto & p : path_with_current_velocity.points) { + p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); + } + + { + debug_data_.path_with_planned_velocity = path_with_current_velocity; + } + + return isSafePath(path_with_current_velocity, check_lanes, debug); +} + +bool AvoidanceModule::isSafePath( + const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const +{ + if (path.points.empty()) { + return true; + } + + const auto path_with_time = [&path]() { + std::vector> ret{}; + + float travel_time = 0.0; + ret.emplace_back(path.points.front(), travel_time); + + for (size_t i = 1; i < path.points.size(); ++i) { + const auto & p1 = path.points.at(i - 1); + const auto & p2 = path.points.at(i); + + const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); + const auto ds = calcDistance2d(p1, p2); + + travel_time += ds / v; + + ret.emplace_back(p2, travel_time); + } + + return ret; + }(); + + const auto move_objects = getAdjacentLaneObjects(check_lanes); + + { + debug.unsafe_objects.clear(); + debug.margin_data_array.clear(); + debug.exist_adjacent_objects = !move_objects.empty(); + } + + bool is_safe = true; + for (const auto & p : path_with_time) { + MarginData margin_data{}; + margin_data.pose = getPose(p.first); + + if (p.second > parameters_->safety_check_time_horizon) { + break; + } + + for (const auto & o : move_objects) { + const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); + + if (!is_enough_margin) { + debug.unsafe_objects.push_back(o); + } + + is_safe = is_safe && is_enough_margin; + } + + debug.margin_data_array.push_back(margin_data); + } + + return is_safe; +} + +bool AvoidanceModule::isEnoughMargin( + const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, + MarginData & margin_data) const +{ + const auto & common_param = planner_data_->parameters; + const auto & vehicle_width = common_param.vehicle_width; + const auto & base_link2front = common_param.base_link2front; + const auto & base_link2rear = common_param.base_link2rear; + + const auto p_ref = [this, &p_ego]() { + const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); + return getPose(avoidance_data_.reference_path.points.at(idx)); + }(); + + const auto & v_ego = p_ego.point.longitudinal_velocity_mps; + const auto & v_ego_lon = getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); + const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + + if (!isTargetObjectType(object.object)) { + return true; + } + + // | centerline + // | ^ x + // | +-------+ | + // | | | | + // | | | D1 | D2 D4 + // | | obj |<-->|<---------->|<->| + // | | | D3 | +-------+ + // | | |<----------->| | + // | +-------+ | | | + // | | | ego | + // | | | | + // | | | | + // | | +-------+ + // | y <----+ + // D1: overhang_dist (signed value) + // D2: shift_length (signed value) + // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) + // D4: vehicle_width (unsigned value) + + const auto reliable_path = std::max_element( + object.object.kinematics.predicted_paths.begin(), + object.object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + if (reliable_path == object.object.kinematics.predicted_paths.end()) { + return true; + } + + const auto p_obj = [&t, &reliable_path]() { + boost::optional ret{boost::none}; + + const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); + const auto idx = static_cast(std::floor(t / dt)); + const auto res = t - dt * idx; + + if (idx > reliable_path->path.size() - 2) { + return ret; + } + + const auto & p_src = reliable_path->path.at(idx); + const auto & p_dst = reliable_path->path.at(idx + 1); + ret = calcInterpolatedPose(p_src, p_dst, res / dt); + return ret; + }(); + + if (!p_obj) { + return true; + } + + const auto v_obj_lon = getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); + + const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); + const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; + const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); + + if (lateral_distance > lateral_margin) { + return true; + } + + const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); + const auto is_front_object = lon_deviation > 0.0; + const auto longitudinal_margin = + getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); + const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; + const auto longitudinal_distance = + std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; + + { + margin_data.pose.orientation = p_ref.orientation; + margin_data.enough_lateral_margin = false; + margin_data.longitudinal_distance = + std::min(margin_data.longitudinal_distance, longitudinal_distance); + margin_data.longitudinal_margin = + std::max(margin_data.longitudinal_margin, longitudinal_margin); + margin_data.vehicle_width = vehicle_width; + margin_data.base_link2front = base_link2front; + margin_data.base_link2rear = base_link2rear; + } + + if (longitudinal_distance > longitudinal_margin) { + return true; + } + + return false; +} + +double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const +{ + const auto & p = parameters_; + + if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { + throw std::logic_error("invalid matrix col size."); + } + + if (velocity < p->target_velocity_matrix.front()) { + return p->target_velocity_matrix.at(p->col_size); + } + + if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { + return p->target_velocity_matrix.back(); + } + + for (size_t i = 1; i < p->col_size; ++i) { + if (velocity < p->target_velocity_matrix.at(i)) { + const auto v1 = p->target_velocity_matrix.at(i - 1); + const auto v2 = p->target_velocity_matrix.at(i); + const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); + const auto m2 = p->target_velocity_matrix.at(i + p->col_size); + + const auto v_clamp = std::clamp(velocity, v1, v2); + return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); + } + } + + return p->target_velocity_matrix.back(); +} + +double AvoidanceModule::getRSSLongitudinalDistance( + const double v_ego, const double v_obj, const bool is_front_object) const +{ + const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; + const auto & idling_time = parameters_->safety_check_idling_time; + + const auto opposite_lane_vehicle = v_obj < 0.0; + + /** + * object and ego already pass each other. + * ======================================= + * Ego--> + * --------------------------------------- + * <--Obj + * ======================================= + */ + if (!is_front_object && opposite_lane_vehicle) { + return 0.0; + } + + /** + * object drive opposite direction. + * ======================================= + * Ego--> + * --------------------------------------- + * <--Obj + * ======================================= + */ + if (is_front_object && opposite_lane_vehicle) { + return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + + std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); + } + + /** + * object is in front of ego, and drive same direction. + * ======================================= + * Ego--> + * --------------------------------------- + * Obj--> + * ======================================= + */ + if (is_front_object && !opposite_lane_vehicle) { + return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + + std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - + std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); + } + + /** + * object is behind ego, and drive same direction. + * ======================================= + * Ego--> + * --------------------------------------- + * Obj--> + * ======================================= + */ + if (!is_front_object && !opposite_lane_vehicle) { + return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(v_obj, 2.0) + + std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - + std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); + } + + return 0.0; +} + +lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( + const PathShifter & path_shifter, const double forward_distance, + const double backward_distance) const +{ + const auto & rh = planner_data_->route_handler; + + bool has_left_shift = false; + bool has_right_shift = false; + + for (const auto & sp : path_shifter.getShiftLines()) { + if (sp.end_shift_length > 0.01) { + has_left_shift = true; + continue; + } + + if (sp.end_shift_length < -0.01) { + has_right_shift = true; + continue; + } + } + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); + + lanelet::ConstLanelets check_lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (has_left_shift && opt_left_lane) { + check_lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (has_right_shift && opt_right_lane) { + check_lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (has_right_shift && !right_opposite_lanes.empty()) { + check_lanes.push_back(right_opposite_lanes.front()); + } + } + + return check_lanes; +} + +ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( + const lanelet::ConstLanelets & adjacent_lanes) const +{ + ObjectDataArray objects; + for (const auto & o : avoidance_data_.other_objects) { + if (isCentroidWithinLanelets(o.object, adjacent_lanes)) { + objects.push_back(o); + } + } + + return objects; +} + // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep // extending during avoidance module // TODO(murooka) freespace during turning in intersection where there is no neighbour lanes @@ -2865,9 +3277,13 @@ void AvoidanceModule::setDebugData( using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; + using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; + using marker_utils::avoidance_marker::createPredictedVehiclePositions; using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; + using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray; + using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; debug_marker_.markers.clear(); @@ -2886,6 +3302,10 @@ void AvoidanceModule::setDebugData( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); + add(createPredictedVehiclePositions( + debug.path_with_planned_velocity, "predicted_vehicle_positions")); + const auto & path = data.reference_path; add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); @@ -2900,6 +3320,9 @@ void AvoidanceModule::setDebugData( add(createOverhangFurthestLineStringMarkerArray( *debug.farthest_linestring_from_overhang, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0)); + add(createUnavoidableObjectsMarkerArray(debug.unavoidable_objects, "unavoidable_objects")); + add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); + // parent object info addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index ffe2d0a4b83a2..7e378dda5954a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utilities.hpp" #include +#include #include #include @@ -34,6 +35,7 @@ namespace behavior_path_planner using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::pose2transform; @@ -496,4 +498,65 @@ void generateDrivableArea( } } +double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) +{ + return v * std::cos(calcYawDeviation(p_ref, p_target)); +} + +bool isCentroidWithinLanelets( + const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) +{ + if (target_lanelets.empty()) { + return false; + } + + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + + for (const auto & llt : target_lanelets) { + if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { + return true; + } + } + + return false; +} + +lanelet::ConstLanelets getTargetLanelets( + const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, + const double left_offset, const double right_offset) +{ + const auto & rh = planner_data->route_handler; + + lanelet::ConstLanelets target_lanelets{}; + for (const auto & lane : route_lanelets) { + auto l_offset = 0.0; + auto r_offset = 0.0; + + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (opt_left_lane) { + target_lanelets.push_back(opt_left_lane.get()); + } else { + l_offset = left_offset; + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (opt_right_lane) { + target_lanelets.push_back(opt_right_lane.get()); + } else { + r_offset = right_offset; + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (!right_opposite_lanes.empty()) { + target_lanelets.push_back(right_opposite_lanes.front()); + } + + const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); + target_lanelets.push_back(expand_lane); + } + + return target_lanelets; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index c6ded01021c33..86afb38acca38 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -28,10 +28,12 @@ namespace marker_utils::avoidance_marker using behavior_path_planner::AvoidLine; using behavior_path_planner::util::shiftPose; using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::getPose; using visualization_msgs::msg::Marker; namespace @@ -75,21 +77,58 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); for (const auto & object : objects) { - marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" + << "lateral: " << object.lateral << " [-]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + + { + marker.id = uuidToInt32(object.object.object_id); + marker.pose.position.z += 2.0; + std::ostringstream string_stream; + string_stream << object.reason; + marker.text = string_stream.str(); + marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker.scale = createMarkerScale(0.6, 0.6, 0.6); + marker.ns = ns + "_reason"; + msg.markers.push_back(marker); + } + } + + return msg; +} + +} // namespace + +MarkerArray createEgoStatusMarkerArray( + const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = p_ego; + + { std::ostringstream string_stream; - string_stream << std::fixed << std::setprecision(2); - string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral: " << object.lateral << " [-]"; + string_stream << std::fixed << std::setprecision(2) << std::boolalpha; + string_stream << "avoid_now:" << data.avoiding_now << "," + << "safe:" << data.safe; marker.text = string_stream.str(); + msg.markers.push_back(marker); } return msg; } -} // namespace - MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) @@ -140,6 +179,68 @@ MarkerArray createAvoidLineMarkerArray( return msg; } +MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + auto p_marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::POINTS, createMarkerScale(0.4, 0.4, 0.0), + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + const auto pushPointMarker = [&](const Pose & p, const double t) { + const auto r = t > 10.0 ? 1.0 : t / 10.0; + p_marker.points.push_back(p.position); + p_marker.colors.push_back(createMarkerColor(r, 1.0 - r, 0.0, 0.999)); + }; + + auto t_marker = createDefaultMarker( + "map", current_time, ns + "_text", 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + const auto pushTextMarker = [&](const Pose & p, const double t, const double d, const double v) { + t_marker.id++; + t_marker.pose = p; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "t[s]: " << t << "\n" + << "d[m]: " << d << "\n" + << "v[m/s]: " << v; + t_marker.text = string_stream.str(); + msg.markers.push_back(t_marker); + }; + + constexpr double dt_save = 1.0; + double t_save = 0.0; + double t_sum = 0.0; + double d_sum = 0.0; + + if (path.points.empty()) { + return msg; + } + + for (size_t i = 1; i < path.points.size(); ++i) { + const auto & p1 = path.points.at(i - 1); + const auto & p2 = path.points.at(i); + const auto ds = calcDistance2d(p1, p2); + + if (t_save < t_sum + 1e-3) { + pushPointMarker(getPose(p1), t_sum); + pushTextMarker(getPose(p1), t_sum, d_sum, p1.point.longitudinal_velocity_mps); + t_save += dt_save; + } + + const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); + + t_sum += ds / v; + d_sum += ds; + } + + msg.markers.push_back(p_marker); + + return msg; +} + MarkerArray createTargetObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { @@ -195,6 +296,26 @@ MarkerArray createOtherObjectsMarkerArray( return msg; } +MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + return createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); +} + +MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), + createMarkerColor(1.0, 0.0, 1.0, 0.9)), + &msg); + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + + return msg; +} + MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { 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 b8924326177d0..4452c36522546 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 @@ -413,8 +413,8 @@ bool isLaneChangePathSafe( // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well - const auto target_lane_object_indices = - util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); + const auto [target_lane_object_indices, others] = + util::separateObjectIndicesByLanelets(*dynamic_objects, target_lanes); // find objects in current lane const double check_distance = common_parameters.forward_path_length; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index 76ccead98268e..a080eefdf64f7 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -52,8 +52,8 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with objects in shoulder lanes const auto arc_path = planner_.getArcPath(); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes); if (util::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) { return {}; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index d16d69839ba36..385bbe6486edd 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -56,8 +56,8 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) } // extract objects in shoulder lane for collision check - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*dynamic_objects, shoulder_lanes); // get safe path for (auto & pull_out_path : pull_out_paths) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index c75d216730e50..1e17ba935a6ed 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -63,8 +63,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto shoulder_lane_objects = - util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); std::vector original_search_poses{}; for (size_t goal_id = 0; goal_id < center_line_path.points.size(); ++goal_id) { diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 37aaef1c18043..9ef2139e9210c 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -552,27 +552,30 @@ std::vector filterObjectIndicesByLanelets( return indices; } -// works with random lanelets -std::vector filterObjectIndicesByLanelets( +std::pair, std::vector> separateObjectIndicesByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { - std::vector indices; if (target_lanelets.empty()) { return {}; } + std::vector target_indices; + std::vector other_indices; + for (size_t i = 0; i < objects.objects.size(); i++) { // create object polygon const auto & obj = objects.objects.at(i); // create object polygon Polygon2d obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { + if (!util::calcObjectPolygon(obj, &obj_polygon)) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("utilities"), "Failed to calcObjectPolygon...!!!"); continue; } + bool is_filtered_object = false; + for (const auto & llt : target_lanelets) { // create lanelet polygon const auto polygon2d = llt.polygon2d().basicPolygon(); @@ -581,33 +584,47 @@ std::vector filterObjectIndicesByLanelets( continue; } Polygon2d lanelet_polygon; - lanelet_polygon.outer().reserve(polygon2d.size() + 1); for (const auto & lanelet_point : polygon2d) { lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { - indices.push_back(i); + target_indices.push_back(i); + is_filtered_object = true; break; } } + + if (!is_filtered_object) { + other_indices.push_back(i); + } } - return indices; + + return std::make_pair(target_indices, other_indices); } -PredictedObjects filterObjectsByLanelets( +std::pair separateObjectsByLanelets( const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) { - PredictedObjects filtered_objects; - const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets); - filtered_objects.objects.reserve(indices.size()); - for (const size_t i : indices) { - filtered_objects.objects.push_back(objects.objects.at(i)); + PredictedObjects target_objects; + PredictedObjects other_objects; + + const auto [target_indices, other_indices] = + separateObjectIndicesByLanelets(objects, target_lanelets); + + target_objects.objects.reserve(target_indices.size()); + other_objects.objects.reserve(other_indices.size()); + + for (const size_t i : target_indices) { + target_objects.objects.push_back(objects.objects.at(i)); + } + + for (const size_t i : other_indices) { + other_objects.objects.push_back(objects.objects.at(i)); } - return filtered_objects; + + return std::make_pair(target_objects, other_objects); } bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon)