From ac9dd5095ab1f63ae6cd2e6f0b7265f8edaf41f3 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 2 Aug 2023 15:31:54 +0900 Subject: [PATCH] feat(avoidance): create detection area from latest generated path Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 2 - .../utils/avoidance/avoidance_module_data.hpp | 9 +- .../utils/avoidance/utils.hpp | 6 + .../src/marker_utils/avoidance/debug.cpp | 11 ++ .../avoidance/avoidance_module.cpp | 9 +- .../src/scene_module/avoidance/manager.cpp | 4 - .../src/scene_module/lane_change/manager.cpp | 4 - .../src/utils/avoidance/utils.cpp | 106 ++++++++++++++++++ 8 files changed, 127 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index db2436f796dfe..1b85079c5a41b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -4,8 +4,6 @@ avoidance: resample_interval_for_planning: 0.3 # [m] resample_interval_for_output: 4.0 # [m] - detection_area_right_expand_dist: 0.0 # [m] - detection_area_left_expand_dist: 1.0 # [m] drivable_area_right_bound_offset: 0.0 # [m] drivable_area_left_bound_offset: 0.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 2b1a9cf1b82d4..ab75f6b84fae7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -79,12 +79,6 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // lanelet expand length for right side to find avoidance target vehicles - double detection_area_right_expand_dist = 0.0; - - // lanelet expand length for left side to find avoidance target vehicles - double detection_area_left_expand_dist = 1.0; - // enable avoidance to be perform only in lane with same direction bool use_adjacent_lane{true}; @@ -524,9 +518,10 @@ using MarginDataArray = std::vector; */ struct DebugData { - std::shared_ptr expanded_lanelets; std::shared_ptr current_lanelets; + geometry_msgs::msg::Polygon ego_polygon; + lanelet::ConstLineStrings3d bounds; AvoidLineArray current_shift_lines; // in path shifter diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 8c229b4f26949..5f1ccbcc9aedf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include namespace behavior_path_planner::utils::avoidance @@ -160,6 +161,11 @@ ExtendedPredictedObject transform( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 061cb10883f6c..dc132396ee9be 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -244,6 +244,17 @@ MarkerArray createSafetyCheckMarkerArray( msg.markers.push_back(marker); } + { + auto marker = createDefaultMarker( + "map", current_time, "ego_attension_area", 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + for (const auto & p : data.ego_polygon.points) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + if (state == AvoidanceState::YIELD) { return msg; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 45a26cfb29f48..39ea282691a55 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -279,12 +279,9 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto expanded_lanelets = getTargetLanelets( - planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist, - parameters_->detection_area_right_expand_dist * (-1.0)); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + utils::avoidance::separateObjectsByPath( + helper_.getPreviousSplineShiftPath().path, planner_data_, data, parameters_, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; @@ -311,7 +308,6 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // debug { debug.current_lanelets = std::make_shared(data.current_lanelets); - debug.expanded_lanelets = std::make_shared(expanded_lanelets); std::vector debug_info_array; const auto append = [&](const auto & o) { @@ -2729,7 +2725,6 @@ void AvoidanceModule::updateDebugMarker( add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); add(createOtherObjectsMarkerArray( data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index ec2ede8373831..c21e3e0ebfdb7 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -50,10 +50,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_bound_clipping = get_parameter(node, ns + "enable_bound_clipping"); p.enable_update_path_when_object_is_gone = get_parameter(node, ns + "enable_update_path_when_object_is_gone"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 18c669a207095..75e5ecb40f78a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -193,10 +193,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( get_parameter(node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = get_parameter(node, ns + "resample_interval_for_output"); - p.detection_area_right_expand_dist = - get_parameter(node, ns + "detection_area_right_expand_dist"); - p.detection_area_left_expand_dist = - get_parameter(node, ns + "detection_area_left_expand_dist"); p.enable_update_path_when_object_is_gone = get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 58beaf9cf3a87..bdec012371a01 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -142,6 +142,59 @@ double calcSignedArcLengthToFirstNearestPoint( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } + +geometry_msgs::msg::Polygon createVehiclePolygon( + const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) +{ + const auto & i = vehicle_info; + const auto & front_m = i.max_longitudinal_offset_m; + const auto & width_m = i.vehicle_width_m / 2.0 + offset; + const auto & back_m = i.rear_overhang_m; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(front_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(front_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-back_m, -width_m, 0.0)); + + return polygon; +} + +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Polygon & base_polygon) +{ + Polygon2d one_step_polygon{}; + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p_front); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p_back); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + Polygon2d hull_polygon{}; + boost::geometry::convex_hull(one_step_polygon, hull_polygon); + boost::geometry::correct(hull_polygon); + + return hull_polygon; +} } // namespace bool isOnRight(const ObjectData & obj) @@ -1544,4 +1597,57 @@ std::vector getSafetyCheckTargetObjects( return target_objects; } + +std::pair separateObjectsByPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const AvoidancePlanningData & data, const std::shared_ptr & parameters, + DebugData & debug) +{ + PredictedObjects target_objects; + PredictedObjects other_objects; + + double max_offset = 0.0; + for (const auto & object_parameter : parameters->object_parameters) { + const auto p = object_parameter.second; + const auto offset = p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + max_offset = std::max(max_offset, offset); + } + + const auto ego_polygon = createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); + const auto ego_idx = planner_data->findEgoIndex(path.points); + + Polygon2d attention_area; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p_ego_front = path.points.at(i).point.pose; + const auto & p_ego_back = path.points.at(i + 1).point.pose; + + const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + if (distance_from_ego > parameters->object_check_forward_distance) { + break; + } + + const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + + std::vector unions; + boost::geometry::union_(attention_area, ego_one_step_polygon, unions); + if (!unions.empty()) { + attention_area = unions.front(); + boost::geometry::correct(attention_area); + } + } + + debug.ego_polygon = toMsg(attention_area, data.reference_pose.position.z); + + const auto objects = planner_data->dynamic_object->objects; + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + if (boost::geometry::disjoint(obj_polygon, attention_area)) { + other_objects.objects.push_back(object); + } else { + target_objects.objects.push_back(object); + } + }); + + return std::make_pair(target_objects, other_objects); +} } // namespace behavior_path_planner::utils::avoidance