Skip to content

Commit

Permalink
feat(dynamic_avoidance): ignore cut-out vehicle (#4049)
Browse files Browse the repository at this point in the history 
* feat(dynamic_avoidance): ignore cut-out vehicle

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update cut out decision policy

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Jun 23, 2023
1 parent 25f9d6a commit e70df12
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 36 deletions.
8 changes: 5 additions & 3 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
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
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

0 comments on commit e70df12

Please sign in to comment.