Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(dynamic_avoidance): ignore cut-out vehicle #4049

Merged
merged 2 commits into from
Jun 23, 2023
Merged

feat(dynamic_avoidance): ignore cut-out vehicle #4049

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
45dea57
feat(dynamic_avoidance): ignore cut-out vehicle
takayuki5168 May 30, 2023
8ebf417
update cut out decision policy
takayuki5168 Jun 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface
struct DynamicAvoidanceObject
{
DynamicAvoidanceObject(
const PredictedObject & predicted_object, const double arg_path_projected_vel)
const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel)
: uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
path_projected_vel(arg_path_projected_vel),
vel(arg_vel),
lat_vel(arg_lat_vel),
shape(predicted_object.shape)
{
for (const auto & path : predicted_object.kinematics.predicted_paths) {
Expand All @@ -82,7 +83,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface

std::string uuid;
geometry_msgs::msg::Pose pose;
double path_projected_vel;
double vel;
double lat_vel;
autoware_auto_perception_msgs::msg::Shape shape;
std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};

Expand Down
89 changes: 56 additions & 33 deletions ...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,20 +73,6 @@ geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d &
return geom_obj_point;
}

double calcObstacleProjectedVelocity(
const std::vector<PathPointWithLaneId> & path_points, const PredictedObject & object)
{
const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;

const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position);

const double obj_yaw = tf2::getYaw(obj_pose.orientation);
const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation);

return obj_vel * std::cos(obj_yaw - path_yaw);
}

std::pair<double, double> getMinMaxValues(const std::vector<double> & vec)
{
const size_t min_idx = std::distance(vec.begin(), std::min_element(vec.begin(), vec.end()));
Expand Down Expand Up @@ -142,6 +128,21 @@ std::optional<T> getObjectFromUuid(const std::vector<T> & objects, const std::st
}
return *itr;
}

std::pair<double, double> projectObstacleVelocityToTrajectory(
const std::vector<PathPointWithLaneId> & path_points, const PredictedObject & object)
{
const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;

const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position);

const double obj_yaw = tf2::getYaw(obj_pose.orientation);
const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation);

return std::make_pair(
obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw));
}
} // namespace

#ifdef USE_OLD_ARCHITECTURE
Expand Down Expand Up @@ -321,14 +322,14 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const
continue;
}

const double path_projected_vel =
calcObstacleProjectedVelocity(prev_module_path->points, predicted_object);
const auto [tangent_vel, normal_vel] =
projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object);
// check if velocity is high enough
if (std::abs(path_projected_vel) < parameters_->min_obstacle_vel) {
if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) {
continue;
}

input_objects.push_back(DynamicAvoidanceObject(predicted_object, path_projected_vel));
input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel));
}

// 2. calculate target lanes to filter obstacles
Expand All @@ -338,7 +339,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const
const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes);
const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes);

// 4. check if object will cut into the ego lane.
// 4. check if object will cut into the ego lane or cut out to the next lane.
// NOTE: The oncoming object will be ignored.
constexpr double epsilon_path_lat_diff = 0.3;
std::vector<DynamicAvoidanceObjectCandidate> output_objects_candidate;
Expand All @@ -352,7 +353,7 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const

// Ignore object that will cut into the ego lane
const bool will_object_cut_in = [&]() {
if (object.path_projected_vel < 0) {
if (object.vel < 0) {
// Ignore oncoming object
return false;
}
Expand All @@ -370,6 +371,29 @@ DynamicAvoidanceModule::calcTargetObjectsCandidate() const
continue;
}

// Ignore object that will cut out to the next lane
const bool will_object_cut_out = [&]() {
if (object.vel < 0) {
// Ignore oncoming object
return false;
}

constexpr double object_lat_vel_thresh = 0.3;
if (is_left) {
if (object_lat_vel_thresh < object.lat_vel) {
return true;
}
} else {
if (object.lat_vel < -object_lat_vel_thresh) {
return true;
}
}
return false;
}();
if (will_object_cut_out) {
continue;
}

// get previous object if it exists
const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid(
prev_target_objects_candidate_, object.uuid);
Expand Down Expand Up @@ -489,7 +513,7 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
getMinMaxValues(obj_lon_offset_vec);

// calculate time to collision and apply it to drivable area extraction
const double relative_velocity = getEgoSpeed() - object.path_projected_vel;
const double relative_velocity = getEgoSpeed() - object.vel;
const double time_to_collision = [&]() {
const auto prev_module_path = getPreviousModuleOutput().path;
const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points);
Expand All @@ -505,19 +529,18 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
return std::nullopt;
}

if (0 <= object.path_projected_vel) {
if (0 <= object.vel) {
const double limited_time_to_collision =
std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision);
return std::make_pair(
raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision,
raw_max_obj_lon_offset + object.path_projected_vel * limited_time_to_collision);
raw_min_obj_lon_offset + object.vel * limited_time_to_collision,
raw_max_obj_lon_offset + object.vel * limited_time_to_collision);
}

const double limited_time_to_collision =
std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision);
return std::make_pair(
raw_min_obj_lon_offset + object.path_projected_vel * limited_time_to_collision,
raw_max_obj_lon_offset);
raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset);
}();

if (!obj_lon_offset) {
Expand All @@ -527,15 +550,15 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
const double max_obj_lon_offset = obj_lon_offset->second;

// calculate bound start and end index
const bool is_object_overtaking = (0.0 <= object.path_projected_vel);
const bool is_object_overtaking = (0.0 <= object.vel);
const double start_length_to_avoid =
std::abs(object.path_projected_vel) *
(is_object_overtaking ? parameters_->start_duration_to_avoid_overtaking_object
: parameters_->start_duration_to_avoid_oncoming_object);
std::abs(object.vel) * (is_object_overtaking
? parameters_->start_duration_to_avoid_overtaking_object
: parameters_->start_duration_to_avoid_oncoming_object);
const double end_length_to_avoid =
std::abs(object.path_projected_vel) * (is_object_overtaking
? parameters_->end_duration_to_avoid_overtaking_object
: parameters_->end_duration_to_avoid_oncoming_object);
std::abs(object.vel) * (is_object_overtaking
? parameters_->end_duration_to_avoid_overtaking_object
: parameters_->end_duration_to_avoid_oncoming_object);
const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
obj_seg_idx, min_obj_lon_offset - start_length_to_avoid, path_with_backward_margin.points);
const size_t updated_obj_seg_idx =
Expand Down