From cb900bb2f73d8e9ddbce95c8c6cf2074a6583f27 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 7 Aug 2023 10:35:52 +0900 Subject: [PATCH] feat(dynamic_avoidance): precise cut-out decision (#4527) * feat(dynamic_avoidance): precise cut-out decision Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 47 ++++-- .../dynamic_avoidance_module.cpp | 158 ++++++++++++------ .../dynamic_avoidance/manager.cpp | 11 ++ 3 files changed, 154 insertions(+), 62 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 71cfddaa24d56..ab00bba507ee0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -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}; @@ -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 & 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); @@ -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 latest_time_inside_ego_path{std::nullopt}; + std::vector predicted_paths{}; + MinMaxValue lon_offset_to_avoid; MinMaxValue lat_offset_to_avoid; - std::vector 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 @@ -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 current_uuids_; // NOTE: positive is for meeting entrying condition, and negative is for exiting. @@ -251,7 +274,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface const std::vector & 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 & prev_object) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; double calcTimeToCollision( @@ -260,7 +284,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional> calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( - const std::vector & ego_path, const PredictedObject & object) const; + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( const std::vector & path_points_for_object_polygon, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 689f68616d32d..04b4f1782ad51 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -181,6 +181,20 @@ bool isLeft( } return false; } + +template +std::optional getObstacleFromUuid( + const std::vector & 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( @@ -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() @@ -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) && @@ -368,46 +387,80 @@ 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 { + 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, @@ -415,14 +468,13 @@ void DynamicAvoidanceModule::updateTargetObjects() 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.", @@ -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> DynamicAvoidanceModule::calcPathForObjectPolygon() @@ -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 & 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; } } @@ -643,12 +700,11 @@ std::pair DynamicAvoidanceModule } DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( - const std::vector & ego_path, const PredictedObject & object) const + const std::vector & 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 obj_lat_offset_vec; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 328639513af05..87525068c8c34 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -60,6 +60,11 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_lon_offset_ego_to_cut_in_object = node->declare_parameter(ns + "cut_in_object.min_lon_offset_ego_to_object"); + p.max_time_from_outside_ego_path_for_cut_out = + node->declare_parameter(ns + "cut_out_object.max_time_from_outside_ego_path"); + p.min_cut_out_object_lat_vel = + node->declare_parameter(ns + "cut_out_object.min_object_lat_vel"); + p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); @@ -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( + parameters, ns + "cut_out_object.max_time_from_outside_ego_path", + p->max_time_from_outside_ego_path_for_cut_out); + updateParam( + parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel); + updateParam( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle);