Skip to content

Commit

Permalink
feat(dynamic_avoidance): precise cut-out decision (autowarefoundation…
Browse files Browse the repository at this point in the history 
…#4527)

* feat(dynamic_avoidance): precise cut-out decision

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

* update

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

* fix

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

* fix

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

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168 @LeoDriveProject
takayuki5168 authored and LeoDriveProject committed Aug 16, 2023
1 parent be9b279 commit cb900bb
Show file tree
Hide file tree
Showing 3 changed files with 154 additions and 62 deletions.
47 changes: 36 additions & 11 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 @@ -64,6 +64,8 @@ struct DynamicAvoidanceParameters

double min_time_to_start_cut_in{0.0};
double min_lon_offset_ego_to_cut_in_object{0.0};
double max_time_from_outside_ego_path_for_cut_out{0.0};
double min_cut_out_object_lat_vel{0.0};
double max_front_object_angle{0.0};
double min_crossing_object_vel{0.0};
double max_crossing_object_angle{0.0};
Expand All @@ -89,17 +91,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface
{
DynamicAvoidanceObject(
const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel,
const bool arg_is_collision_left, const double arg_time_to_collision,
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid)
const bool arg_is_object_on_ego_path,
const std::optional<rclcpp::Time> & arg_latest_time_inside_ego_path)
: uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)),
pose(predicted_object.kinematics.initial_pose_with_covariance.pose),
shape(predicted_object.shape),
vel(arg_vel),
lat_vel(arg_lat_vel),
is_collision_left(arg_is_collision_left),
time_to_collision(arg_time_to_collision),
lon_offset_to_avoid(arg_lon_offset_to_avoid),
lat_offset_to_avoid(arg_lat_offset_to_avoid)
is_object_on_ego_path(arg_is_object_on_ego_path),
latest_time_inside_ego_path(arg_latest_time_inside_ego_path)
{
for (const auto & path : predicted_object.kinematics.predicted_paths) {
predicted_paths.push_back(path);
Expand All @@ -111,11 +111,24 @@ class DynamicAvoidanceModule : public SceneModuleInterface
autoware_auto_perception_msgs::msg::Shape shape;
double vel;
double lat_vel;
bool is_collision_left;
double time_to_collision;
bool is_object_on_ego_path;
std::optional<rclcpp::Time> latest_time_inside_ego_path{std::nullopt};
std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};

MinMaxValue lon_offset_to_avoid;
MinMaxValue lat_offset_to_avoid;
std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};
bool is_collision_left;
bool should_be_avoided{false};

void update(
const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
const bool arg_is_collision_left, const bool arg_should_be_avoided)
{
lon_offset_to_avoid = arg_lon_offset_to_avoid;
lat_offset_to_avoid = arg_lat_offset_to_avoid;
is_collision_left = arg_is_collision_left;
should_be_avoided = arg_should_be_avoided;
}
};

struct TargetObjectsManager
Expand Down Expand Up @@ -205,6 +218,16 @@ class DynamicAvoidanceModule : public SceneModuleInterface
}
return object_map_.at(uuid);
}
void updateObject(
const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
const bool should_be_avoided)
{
if (object_map_.count(uuid) != 0) {
object_map_.at(uuid).update(
lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided);
}
}

std::vector<std::string> current_uuids_;
// NOTE: positive is for meeting entrying condition, and negative is for exiting.
Expand Down Expand Up @@ -251,7 +274,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
bool willObjectCutOut(
const double obj_tangent_vel, const double obj_normal_vel, const bool is_collision_left) const;
const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left,
const std::optional<DynamicAvoidanceObject> & prev_object) const;
bool isObjectFarFromPath(
const PredictedObject & predicted_object, const double obj_dist_to_path) const;
double calcTimeToCollision(
Expand All @@ -260,7 +284,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface
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 PredictedObject & object) const;
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_auto_perception_msgs::msg::Shape & obj_shape) const;
MinMaxValue calcMinMaxLongitudinalOffsetToAvoid(
const std::vector<PathPointWithLaneId> & path_points_for_object_polygon,
const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel,
Expand Down
158 changes: 107 additions & 51 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 @@ -181,6 +181,20 @@ bool isLeft(
}
return false;
}

template <typename T>
std::optional<T> getObstacleFromUuid(
const std::vector<T> & obstacles, const std::string & target_uuid)
{
const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) {
return obstacle.uuid == target_uuid;
});

if (itr == obstacles.end()) {
return std::nullopt;
}
return *itr;
}
} // namespace

DynamicAvoidanceModule::DynamicAvoidanceModule(
Expand Down Expand Up @@ -230,7 +244,13 @@ void DynamicAvoidanceModule::updateData()
// calculate target objects
updateTargetObjects();

target_objects_ = target_objects_manager_.getValidObjects();
const auto target_objects_candidate = target_objects_manager_.getValidObjects();
target_objects_.clear();
for (const auto & target_object_candidate : target_objects_candidate) {
if (target_object_candidate.should_be_avoided) {
target_objects_.push_back(target_object_candidate);
}
}
}

ModuleStatus DynamicAvoidanceModule::updateState()
Expand Down Expand Up @@ -328,32 +348,31 @@ void DynamicAvoidanceModule::updateTargetObjects()
return;
}

const auto prev_objects = target_objects_manager_.getValidObjects();

// 1. Rough filtering of target objects
target_objects_manager_.initialize();
for (const auto & predicted_object : predicted_objects) {
const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id);
const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
const auto obj_path = *std::max_element(
predicted_object.kinematics.predicted_paths.begin(),
predicted_object.kinematics.predicted_paths.end(),
[](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
const auto prev_object = target_objects_manager_.getValidObject(obj_uuid);
const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);

// 1. check label
// 1.a. check label
const bool is_label_target_obstacle =
isLabelTargetObstacle(predicted_object.classification.front().label);
if (!is_label_target_obstacle) {
continue;
}

// 2. check if velocity is large enough
// 1.b. check if velocity is large enough
const auto [obj_tangent_vel, obj_normal_vel] =
projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object);
if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) {
continue;
}

// 3. check if object is not crossing ego's path
// 1.c. check if object is not crossing ego's path
const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose);
const bool is_obstacle_crossing_path =
parameters_->max_crossing_object_angle < std::abs(obj_angle) &&
Expand All @@ -368,61 +387,94 @@ void DynamicAvoidanceModule::updateTargetObjects()
continue;
}

// 4. check if object is not to be followed by ego
// 1.e. check if object lateral offset to ego's path is small enough
const double obj_dist_to_path = calcDistanceToPath(prev_module_path->points, obj_pose.position);
const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path);
if (is_object_far_from_path) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since lateral offset is large.", obj_uuid.c_str());
continue;
}

// 1.f. calculate the object is on ego's path or not
const bool is_object_on_ego_path =
obj_dist_to_path <
planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path;

// 1.g. calculate latest time inside ego's path
const auto latest_time_inside_ego_path = [&]() -> std::optional<rclcpp::Time> {
if (!prev_object || !prev_object->latest_time_inside_ego_path) {
if (is_object_on_ego_path) {
return clock_->now();
}
return std::nullopt;
}
if (is_object_on_ego_path) {
return clock_->now();
}
return *prev_object->latest_time_inside_ego_path;
}();

const auto target_object = DynamicAvoidanceObject(
predicted_object, obj_tangent_vel, obj_normal_vel, is_object_on_ego_path,
latest_time_inside_ego_path);
target_objects_manager_.updateObject(obj_uuid, target_object);
}
target_objects_manager_.finalize();

// 2. Precise filtering of target objects and check if they should be avoided
for (const auto & object : target_objects_manager_.getValidObjects()) {
const auto obj_uuid = object.uuid;
const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);
const auto obj_path = *std::max_element(
object.predicted_paths.begin(), object.predicted_paths.end(),
[](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });

// 2.a. check if object is not to be followed by ego
const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, object.pose);
const bool is_object_aligned_to_path =
std::abs(obj_angle) < parameters_->max_front_object_angle ||
M_PI - parameters_->max_front_object_angle < std::abs(obj_angle);
if (is_object_on_ego_path && is_object_aligned_to_path) {
if (object.is_object_on_ego_path && is_object_aligned_to_path) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since it is to be followed.", obj_uuid.c_str());
continue;
}

// 5. check if object lateral offset to ego's path is large enough
const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path);
if (is_object_far_from_path) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since lateral offset is large.", obj_uuid.c_str());
continue;
}

// 6. calculate which side object exists against ego's path
const bool is_object_left = isLeft(prev_module_path->points, obj_pose.position);
// 2.b. calculate which side object exists against ego's path
const bool is_object_left = isLeft(prev_module_path->points, object.pose.position);
const auto lat_lon_offset =
getLateralLongitudinalOffset(prev_module_path->points, predicted_object);
getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape);

// 7. check if object will not cut in or cut out
// 2.c. check if object will not cut in
const bool will_object_cut_in =
willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel, lat_lon_offset);
const bool will_object_cut_out =
willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_object_left);
willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset);
if (will_object_cut_in) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since it will cut in.", obj_uuid.c_str());
continue;
}

// 2.d. check if object will not cut out
const bool will_object_cut_out =
willObjectCutOut(object.vel, object.lat_vel, is_object_left, prev_object);
if (will_object_cut_out) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
"[DynamicAvoidance] Ignore obstacle (%s) since it will cut out.", obj_uuid.c_str());
continue;
}

// 8. check if time to collision
// 2.e. check if time to collision
const double time_to_collision =
calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel);
calcTimeToCollision(prev_module_path->points, object.pose, object.vel);
if (
(0 <= obj_tangent_vel &&
(0 <= object.vel &&
parameters_->max_time_to_collision_overtaking_object < time_to_collision) ||
(obj_tangent_vel <= 0 &&
parameters_->max_time_to_collision_oncoming_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 is large.",
Expand All @@ -438,27 +490,24 @@ void DynamicAvoidanceModule::updateTargetObjects()
continue;
}

// 9. calculate which side object will be against ego's path
// 2.f. calculate which side object will be against ego's path
const auto future_obj_pose =
object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision);
const bool is_collision_left = future_obj_pose
? isLeft(prev_module_path->points, future_obj_pose->position)
: is_object_left;

// 10. calculate longitudinal and lateral offset to avoid
const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, predicted_object.shape);
// 2.g. calculate longitudinal and lateral offset to avoid
const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid(
*path_points_for_object_polygon, obj_pose, obj_points, obj_tangent_vel, time_to_collision);
*path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision);
const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid(
*path_points_for_object_polygon, obj_points, is_collision_left, prev_object);

const auto target_object = DynamicAvoidanceObject(
predicted_object, obj_tangent_vel, obj_normal_vel, is_collision_left, time_to_collision,
lon_offset_to_avoid, lat_offset_to_avoid);

target_objects_manager_.updateObject(obj_uuid, target_object);
const bool should_be_avoided = true;
target_objects_manager_.updateObject(
obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided);
}
target_objects_manager_.finalize();
}

std::optional<std::vector<PathPointWithLaneId>> DynamicAvoidanceModule::calcPathForObjectPolygon()
Expand Down Expand Up @@ -582,20 +631,28 @@ bool DynamicAvoidanceModule::willObjectCutIn(
}

bool DynamicAvoidanceModule::willObjectCutOut(
const double obj_tangent_vel, const double obj_normal_vel, const bool is_collision_left) const
const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left,
const std::optional<DynamicAvoidanceObject> & prev_object) const
{
// Ignore oncoming object
if (obj_tangent_vel < 0) {
return false;
}

constexpr double object_lat_vel_thresh = 0.3;
if (is_collision_left) {
if (object_lat_vel_thresh < obj_normal_vel) {
if (prev_object && prev_object->latest_time_inside_ego_path) {
if (
parameters_->max_time_from_outside_ego_path_for_cut_out <
(clock_->now() - *prev_object->latest_time_inside_ego_path).seconds()) {
return false;
}
}

if (is_object_left) {
if (parameters_->min_cut_out_object_lat_vel < obj_normal_vel) {
return true;
}
} else {
if (obj_normal_vel < -object_lat_vel_thresh) {
if (obj_normal_vel < -parameters_->min_cut_out_object_lat_vel) {
return true;
}
}
Expand Down Expand Up @@ -643,12 +700,11 @@ std::pair<lanelet::ConstLanelets, lanelet::ConstLanelets> DynamicAvoidanceModule
}

DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedObject & object) const
const std::vector<PathPointWithLaneId> & ego_path, const geometry_msgs::msg::Pose & obj_pose,
const autoware_auto_perception_msgs::msg::Shape & obj_shape) const
{
const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;

const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position);
const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, object.shape);
const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape);

// TODO(murooka) calculation is not so accurate.
std::vector<double> obj_lat_offset_vec;
Expand Down
11 changes: 11 additions & 0 deletions planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,11 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
p.min_lon_offset_ego_to_cut_in_object =
node->declare_parameter<double>(ns + "cut_in_object.min_lon_offset_ego_to_object");

p.max_time_from_outside_ego_path_for_cut_out =
node->declare_parameter<double>(ns + "cut_out_object.max_time_from_outside_ego_path");
p.min_cut_out_object_lat_vel =
node->declare_parameter<double>(ns + "cut_out_object.min_object_lat_vel");

p.max_front_object_angle =
node->declare_parameter<double>(ns + "front_object.max_object_angle");

Expand Down Expand Up @@ -137,6 +142,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
parameters, ns + "cut_in_object.min_lon_offset_ego_to_object",
p->min_lon_offset_ego_to_cut_in_object);

updateParam<double>(
parameters, ns + "cut_out_object.max_time_from_outside_ego_path",
p->max_time_from_outside_ego_path_for_cut_out);
updateParam<double>(
parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel);

updateParam<double>(
parameters, ns + "front_object.max_object_angle", p->max_front_object_angle);

Expand Down

0 comments on commit cb900bb

Please sign in to comment.