Skip to content

Commit

Permalink
feat(dynamic_avoidance): change the logic of longitudinal distance to… (
Browse files Browse the repository at this point in the history 
#5947)

* feat(dynamic_avoidance): change the logic of longitudinal distance to avoid

Signed-off-by: Takayuki Murooka <[email protected]>

* fix typo

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168
takayuki5168 authored Dec 25, 2023
1 parent 9f656e3 commit 9ccb981
Show file tree
Hide file tree
Showing 4 changed files with 202 additions and 76 deletions.
3 changes: 3 additions & 0 deletions planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@
min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle.
max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value.

stopped_object:
max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value.

drivable_area_generation:
polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base"
object_path_base:
Expand Down
18 changes: 13 additions & 5 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 @@ -87,6 +87,7 @@ struct DynamicAvoidanceParameters
double max_overtaking_crossing_object_angle{0.0};
double min_oncoming_crossing_object_vel{0.0};
double max_oncoming_crossing_object_angle{0.0};
double max_stopped_object_vel{0.0};

// drivable area generation
PolygonGenerationMethod polygon_generation_method{};
Expand All @@ -106,6 +107,12 @@ struct DynamicAvoidanceParameters
double end_duration_to_avoid_oncoming_object{0.0};
};

struct TimeWhileCollision
{
double time_to_start_collision;
double time_to_end_collision;
};

class DynamicAvoidanceModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -324,22 +331,23 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const;
bool isObjectFarFromPath(
const PredictedObject & predicted_object, const double obj_dist_to_path) const;
double calcTimeToCollision(
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
TimeWhileCollision calcTimeWhileCollision(
const std::vector<PathPointWithLaneId> & ego_path, const double obj_tangent_vel,
const LatLonOffset & lat_lon_offset) const;
std::optional<std::pair<size_t, size_t>> calcCollisionSection(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const;
LatLonOffset getLateralLongitudinalOffset(
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
double calcValidLengthToAvoid(
const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
const autoware_auto_perception_msgs::msg::Shape & obj_shape,
const bool is_object_same_direction) const;
MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
const double time_to_collision) const;
const TimeWhileCollision & time_while_collision) const;
std::optional<MinMaxValue> calcMinMaxLateralOffsetToAvoid(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const Polygon2d & obj_points, const double obj_vel, const bool is_collision_left,
Expand Down
251 changes: 180 additions & 71 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 @@ -570,25 +570,30 @@ void DynamicAvoidanceModule::updateTargetObjects()
// }

// 2.e. check time to collision
const double time_to_collision =
calcTimeToCollision(input_path.points, object.pose, object.vel, lat_lon_offset);
if (
(0 <= object.vel &&
parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
(object.vel <= 0 && parameters_->max_time_to_collision_oncoming_object < time_to_collision)) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.",
obj_uuid.c_str(), time_to_collision);
continue;
}
if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small negative "
"value.",
obj_uuid.c_str(), time_to_collision);
continue;
const auto time_while_collision =
calcTimeWhileCollision(input_path.points, object.vel, lat_lon_offset);
const double time_to_collision = time_while_collision.time_to_start_collision;
if (parameters_->max_stopped_object_vel < std::hypot(object.vel, object.lat_vel)) {
// NOTE: Only not stopped object is filtered by time to collision.
if (
(0 <= object.vel &&
parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
(object.vel <= 0 &&
parameters_->max_time_to_collision_oncoming_object < time_to_collision)) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is large.",
obj_uuid.c_str(), time_to_collision);
continue;
}
if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since time to collision (%f) is a small "
"negative value.",
obj_uuid.c_str(), time_to_collision);
continue;
}
}

// 2.f. calculate which side object will be against ego's path
Expand Down Expand Up @@ -630,7 +635,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid(
ref_path_points_for_obj_poly, object.pose, obj_points, object.vel, obj_path, object.shape,
time_to_collision);
time_while_collision);
const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid(
ref_path_points_for_obj_poly, obj_points, object.vel, is_collision_left, object.lat_vel,
prev_object);
Expand Down Expand Up @@ -716,26 +721,60 @@ DynamicAvoidanceModule::calcCollisionSection(
return std::make_pair(*collision_start_idx, ego_path.size() - 1);
}

double DynamicAvoidanceModule::calcTimeToCollision(
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const
TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision(
const std::vector<PathPointWithLaneId> & ego_path, const double obj_tangent_vel,
const LatLonOffset & lat_lon_offset) const
{
// Set maximum time-to-collision 0 if the object longitudinally overlaps ego.
// NOTE: This is to avoid objects running right beside ego even if time-to-collision is negative.
const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path);
const double lon_offset_ego_to_obj =
motion_utils::calcSignedArcLength(
ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx) +
lat_lon_offset.max_lon_offset;
const double maximum_time_to_collision =
(0.0 < lon_offset_ego_to_obj) ? 0.0 : -std::numeric_limits<double>::max();

const double lon_offset_ego_to_obj_idx = motion_utils::calcSignedArcLength(
ego_path, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx);
const double relative_velocity = getEgoSpeed() - obj_tangent_vel;
const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position);
const double signed_lon_length = motion_utils::calcSignedArcLength(
ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx);
const double positive_relative_velocity = std::max(relative_velocity, 1.0);
return std::max(maximum_time_to_collision, signed_lon_length / positive_relative_velocity);

const double signed_time_to_start_collision = [&]() {
const double lon_offset_ego_front_to_obj_back = lon_offset_ego_to_obj_idx +
lat_lon_offset.min_lon_offset -
planner_data_->parameters.front_overhang;
const double lon_offset_obj_front_to_ego_back = -lon_offset_ego_to_obj_idx -
lat_lon_offset.max_lon_offset -
planner_data_->parameters.rear_overhang;
if (0.0 < lon_offset_ego_front_to_obj_back) { // The object is ahead of the ego.
return lon_offset_ego_front_to_obj_back / relative_velocity;
} else if (0.0 < lon_offset_obj_front_to_ego_back) { // The ego is ahead of the object.
return lon_offset_obj_front_to_ego_back / -relative_velocity;
}
// The ego and object are colliding.
return 0.0;
}();
const double signed_time_to_end_collision = [&]() {
const double lon_offset_ego_back_to_obj_front = lon_offset_ego_to_obj_idx +
lat_lon_offset.max_lon_offset +
planner_data_->parameters.rear_overhang;
const double lon_offset_obj_back_to_ego_front = -lon_offset_ego_to_obj_idx -
lat_lon_offset.min_lon_offset +
planner_data_->parameters.front_overhang;
if (0.0 < relative_velocity) {
return lon_offset_ego_back_to_obj_front / relative_velocity;
}
return lon_offset_obj_back_to_ego_front / -relative_velocity;
}();

// NOTE: In order to make time_to_start_collision continuous around the relative_velocity is zero.
const double time_to_start_collision = [&]() {
if (signed_time_to_start_collision < 0.0) {
return std::numeric_limits<double>::max();
}
return signed_time_to_start_collision;
}();
const double time_to_end_collision = [&]() {
if (signed_time_to_end_collision < 0.0) {
return std::numeric_limits<double>::max();
}
return signed_time_to_end_collision;
}();

return TimeWhileCollision{time_to_start_collision, time_to_end_collision};
}

bool DynamicAvoidanceModule::isObjectFarFromPath(
Expand Down Expand Up @@ -928,7 +967,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
const std::vector<PathPointWithLaneId> & ref_path_points_for_obj_poly,
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape,
const double time_to_collision) const
const TimeWhileCollision & time_while_collision) const
{
const size_t obj_seg_idx =
motion_utils::findNearestSegmentIndex(ref_path_points_for_obj_poly, obj_pose.position);
Expand All @@ -943,40 +982,54 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(
obj_lon_offset_vec.push_back(lon_offset);
}

const auto raw_obj_lon_offset = getMinMaxValues(obj_lon_offset_vec);
return getMinMaxValues(obj_lon_offset_vec);
}();

if (obj_vel < 0) {
return MinMaxValue{
raw_obj_lon_offset.min_value + obj_vel * time_to_collision, raw_obj_lon_offset.max_value};
}
const double relative_velocity = getEgoSpeed() - obj_vel;

// NOTE: If time to collision is considered here, the ego is close to the object when starting
// avoidance.
// The ego should start avoidance before overtaking.
return raw_obj_lon_offset;
// calculate bound start and end length
const double start_length_to_avoid = [&]() {
if (obj_vel < parameters_->max_stopped_object_vel) {
// The ego and object are the same directional or the object is parked.
return std::min(time_while_collision.time_to_start_collision, 8.0) * std::abs(obj_vel) +
std::abs(relative_velocity) * parameters_->start_duration_to_avoid_oncoming_object;
}
// The ego and object are the opposite directional.
const double obj_acc = -1.0;
const double decel_time = 1.0;
const double obj_moving_dist =
(std::pow(std::max(obj_vel + obj_acc * decel_time, 0.0), 2) - std::pow(obj_vel, 2)) / 2 /
obj_acc;
const double ego_moving_dist = getEgoSpeed() * decel_time;
return std::max(0.0, ego_moving_dist - obj_moving_dist) +
std::abs(relative_velocity) * parameters_->start_duration_to_avoid_overtaking_object;
}();
const double end_length_to_avoid = [&]() {
if (obj_vel < parameters_->max_stopped_object_vel) {
// The ego and object are the same directional or the object is parked.
return std::abs(relative_velocity) * parameters_->end_duration_to_avoid_oncoming_object;
}
// The ego and object are the opposite directional.
return std::min(time_while_collision.time_to_end_collision, 3.0) * obj_vel +
std::abs(relative_velocity) * parameters_->end_duration_to_avoid_overtaking_object;
}();

// calculate bound start and end index
const bool is_object_overtaking = (0.0 <= obj_vel);
// TODO(murooka) use getEgoSpeed() instead of obj_vel
const double start_length_to_avoid =
std::abs(obj_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(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object
: parameters_->end_duration_to_avoid_oncoming_object);

const double valid_length_to_avoid = calcValidLengthToAvoid(obj_path, obj_pose, obj_shape);
if (obj_vel < -0.5) {
// calculate valid path for the forked object's path from the ego's path
if (obj_vel < -parameters_->max_stopped_object_vel) {
const bool is_object_same_direction = false;
const double valid_length_to_avoid =
calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction);
return MinMaxValue{
std::max(obj_lon_offset.min_value - start_length_to_avoid, -valid_length_to_avoid),
obj_lon_offset.min_value + std::max(-start_length_to_avoid, -valid_length_to_avoid),
obj_lon_offset.max_value + end_length_to_avoid};
}
if (0.5 < obj_vel) {
if (parameters_->max_stopped_object_vel < obj_vel) {
const bool is_object_same_direction = true;
const double valid_length_to_avoid =
calcValidLengthToAvoid(obj_path, obj_pose, obj_shape, is_object_same_direction);
return MinMaxValue{
obj_lon_offset.min_value - start_length_to_avoid,
std::min(obj_lon_offset.max_value + end_length_to_avoid, valid_length_to_avoid)};
obj_lon_offset.max_value + std::min(end_length_to_avoid, valid_length_to_avoid)};
}
return MinMaxValue{
obj_lon_offset.min_value - start_length_to_avoid,
Expand All @@ -985,25 +1038,44 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid(

double DynamicAvoidanceModule::calcValidLengthToAvoid(
const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_auto_perception_msgs::msg::Shape & obj_shape) const
const autoware_auto_perception_msgs::msg::Shape & obj_shape,
const bool is_object_same_direction) const
{
const auto input_path_points = getPreviousModuleOutput().path.points;
const auto & input_path_points = getPreviousModuleOutput().path.points;
const size_t obj_seg_idx =
motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position);

const double dist_threshold_paths = planner_data_->parameters.vehicle_width / 2.0 +
parameters_->lat_offset_from_obstacle +
calcObstacleMaxLength(obj_shape);

// crop the ego's path by object position
std::vector<PathPointWithLaneId> cropped_ego_path_points;
if (is_object_same_direction) {
cropped_ego_path_points = std::vector<PathPointWithLaneId>{
input_path_points.begin() + obj_seg_idx, input_path_points.end()};
} else {
cropped_ego_path_points = std::vector<PathPointWithLaneId>{
input_path_points.begin(), input_path_points.begin() + obj_seg_idx + 1 + 1};
std::reverse(cropped_ego_path_points.begin(), cropped_ego_path_points.end());
}
if (cropped_ego_path_points.size() < 2) {
return motion_utils::calcArcLength(obj_path.path);
}

// calculate where the object's path will be forked from (= far from) the ego's path.
std::optional<size_t> last_nearest_ego_path_seg_idx{std::nullopt};
const size_t valid_obj_path_end_idx = [&]() {
int ego_path_idx = obj_seg_idx + 1;
size_t ego_path_seg_idx = 0;
for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) {
bool are_paths_close{false};
for (; 0 < ego_path_idx; --ego_path_idx) {
for (; ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) {
const double dist_to_segment = calcDistanceToSegment(
obj_path.path.at(obj_path_idx).position,
input_path_points.at(ego_path_idx).point.pose.position,
input_path_points.at(ego_path_idx - 1).point.pose.position);
if (
dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 +
parameters_->lat_offset_from_obstacle +
calcObstacleMaxLength(obj_shape)) {
cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position,
cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position);
if (dist_to_segment < dist_threshold_paths) {
last_nearest_ego_path_seg_idx = ego_path_seg_idx;
are_paths_close = true;
break;
}
Expand All @@ -1015,6 +1087,43 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid(
}
return obj_path.path.size() - 1;
}();

// calculate valid length to avoid
if (last_nearest_ego_path_seg_idx && valid_obj_path_end_idx != obj_path.path.size() - 1) {
const auto calc_min_dist = [&](const size_t arg_obj_path_idx) -> std::optional<double> {
std::optional<double> min_dist{std::nullopt};
for (size_t ego_path_seg_idx = *last_nearest_ego_path_seg_idx;
ego_path_seg_idx < cropped_ego_path_points.size() - 1; ++ego_path_seg_idx) {
const double dist_to_segment = calcDistanceToSegment(
obj_path.path.at(arg_obj_path_idx).position,
cropped_ego_path_points.at(ego_path_seg_idx).point.pose.position,
cropped_ego_path_points.at(ego_path_seg_idx + 1).point.pose.position);
if (!min_dist || dist_to_segment < *min_dist) {
min_dist = dist_to_segment;
}
if (min_dist && *min_dist < dist_to_segment) {
return *min_dist;
}
}
return min_dist;
};
const size_t prev_valid_obj_path_end_idx =
(valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx : valid_obj_path_end_idx - 1;
const size_t next_valid_obj_path_end_idx =
(valid_obj_path_end_idx == 0) ? valid_obj_path_end_idx + 1 : valid_obj_path_end_idx;
const auto prev_min_dist = calc_min_dist(prev_valid_obj_path_end_idx);
const auto next_min_dist = calc_min_dist(next_valid_obj_path_end_idx);
if (prev_min_dist && next_min_dist) {
const double segment_length = tier4_autoware_utils::calcDistance2d(
obj_path.path.at(prev_valid_obj_path_end_idx),
obj_path.path.at(next_valid_obj_path_end_idx));
const double partial_segment_length = segment_length *
(dist_threshold_paths - *prev_min_dist) /
(*next_min_dist - *prev_min_dist);
return motion_utils::calcSignedArcLength(obj_path.path, 0, prev_valid_obj_path_end_idx) +
partial_segment_length;
}
}
return motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx);
}

Expand Down
Loading

0 comments on commit 9ccb981

Please sign in to comment.