Skip to content

Commit

Permalink
feat(avoidance): check incomming vehicle (autowarefoundation#4864)
Browse files Browse the repository at this point in the history 
feat(avoidance): check incomming behicle

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Sep 15, 2023
1 parent 6aa1834 commit bd1329f
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 38 deletions.
2 changes: 0 additions & 2 deletions ...ning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons

MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);

MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);

MarkerArray makeOverhangToRoadShoulderMarkerArray(
const behavior_path_planner::ObjectDataArray & objects, std::string && ns);

Expand Down
7 changes: 4 additions & 3 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -556,9 +556,6 @@ struct DebugData
// shift path
std::vector<double> proposed_spline_shift;

// future pose
PathWithLaneId path_with_planned_velocity;

// avoidance require objects
ObjectDataArray unavoidable_objects;

Expand All @@ -568,6 +565,10 @@ struct DebugData
// tmp for plot
PathWithLaneId center_line;

// collision check debug map
utils::path_safety_checker::CollisionCheckDebugMap collision_check;

// debug msg array
AvoidanceDebugMsgArray avoidance_debug_msg_array;
};

Expand Down
6 changes: 0 additions & 6 deletions planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -416,12 +416,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
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 makeOverhangToRoadShoulderMarkerArray(
const ObjectDataArray & objects, std::string && ns)
{
Expand Down
74 changes: 47 additions & 27 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1852,25 +1852,35 @@ bool AvoidanceModule::isSafePath(
avoid_data_, planner_data_, parameters_, is_right_shift.value());

for (const auto & object : safety_check_target_objects) {
auto current_debug_data = marker_utils::createObjectDebug(object);

const auto obj_polygon =
tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape);

const auto is_object_front =
utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);

const auto is_object_incoming =
std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2;

const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
object, parameters_->check_all_predicted_path);

const auto & ego_predicted_path =
is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object;
const auto & ego_predicted_path = is_object_front && !is_object_incoming
? ego_predicted_path_for_front_object
: ego_predicted_path_for_rear_object;

for (const auto & obj_path : obj_predicted_paths) {
CollisionCheckDebug collision{};
if (!utils::path_safety_checker::checkCollision(
shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
hysteresis_factor, collision)) {
hysteresis_factor, current_debug_data.second)) {
marker_utils::updateCollisionCheckDebugMap(
debug.collision_check, current_debug_data, false);

safe_count_ = 0;
return false;
} else {
marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true);
}
}
}
Expand Down Expand Up @@ -2682,12 +2692,14 @@ void AvoidanceModule::updateDebugMarker(
using marker_utils::createShiftGradMarkerArray;
using marker_utils::createShiftLengthMarkerArray;
using marker_utils::createShiftLineMarkerArray;
using marker_utils::showPolygon;
using marker_utils::showPredictedPath;
using marker_utils::showSafetyCheckInfo;
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::createUnsafeObjectsMarkerArray;
using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray;
using tier4_autoware_utils::appendMarkerArray;

Expand All @@ -2711,9 +2723,11 @@ void AvoidanceModule::updateDebugMarker(
add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w));
};

const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) {
add(createOtherObjectsMarkerArray(objects, ns));
};

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));
Expand All @@ -2725,31 +2739,37 @@ void AvoidanceModule::updateDebugMarker(
add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1));

add(createOtherObjectsMarkerArray(
data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD));
add(createOtherObjectsMarkerArray(
data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD));
add(createOtherObjectsMarkerArray(
data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL));
add(createOtherObjectsMarkerArray(
data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE));
add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE));
add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT));
add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject")));
add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser")));
add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea")));
add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance")));
add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold")));

add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang"));
add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0));

add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects"));
// ignore objects
{
addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD);
addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD);
addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE);
addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL);
addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE);
addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT);
addObjects(data.other_objects, std::string("MovingObject"));
addObjects(data.other_objects, std::string("CrosswalkUser"));
addObjects(data.other_objects, std::string("OutOfTargetArea"));
addObjects(data.other_objects, std::string("NotNeedAvoidance"));
addObjects(data.other_objects, std::string("LessThanExecutionThreshold"));
}

// 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);
addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8);
{
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);
addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8);
}

// safety check
{
add(showSafetyCheckInfo(debug.collision_check, "object_debug_info"));
add(showPredictedPath(debug.collision_check, "ego_predicted_path"));
add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation"));
}

// shift length
{
Expand Down

0 comments on commit bd1329f

Please sign in to comment.