diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 4c8fe5c6fa0f5..07847a08c1209 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/scene_intersection.cpp src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp src/scene_intersection_prepare_data.cpp src/scene_intersection_stuck.cpp src/scene_intersection_occlusion.cpp diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 108e021240851..c7f6f62d575a0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -59,6 +59,8 @@ object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 350f6d774f7cf..978855b36c5f6 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -31,8 +31,6 @@ #include #include -namespace behavior_velocity_planner -{ namespace { using tier4_autoware_utils::appendMarkerArray; @@ -40,14 +38,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -158,8 +156,59 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return marker_point; } +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -168,14 +217,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), &debug_marker_array); @@ -183,14 +232,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.adjacent_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -198,7 +247,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -214,7 +263,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.yield_stuck_detect_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), &debug_marker_array); @@ -222,7 +271,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.ego_lane) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } @@ -235,59 +284,58 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - size_t i{0}; - for (const auto & p : debug_data_.candidate_collision_object_polygons) { - appendMarkerArray( - debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, - 0.5), - &debug_marker_array, now); - } - + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, - 0.0, 1.0, 0.0), + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, - 0.99, 0.6), + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - /* appendMarkerArray( - createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - */ if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -297,7 +345,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -314,7 +362,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - createLineMarkerArray( + ::createLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); } @@ -369,11 +417,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = planning_utils::bitShift(module_id_); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp new file mode 100644 index 0000000000000..93a7c2a29d2f7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decision_result.hpp" + +namespace behavior_velocity_planner::intersection +{ +std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "InternalError because " + state.error; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + + state.evasive_report; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index 48b0ecf1349a5..da71168c2c4ca 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -23,16 +23,23 @@ namespace behavior_velocity_planner::intersection { /** - * @struct - * @brief Internal error or ego already passed pass_judge_line + * @brief internal error */ -struct Indecisive +struct InternalError { std::string error; }; /** - * @struct + * @brief + */ +struct OverPassJudge +{ + std::string safety_report; + std::string evasive_report; +}; + +/** * @brief detected stuck vehicle */ struct StuckStop @@ -43,17 +50,16 @@ struct StuckStop }; /** - * @struct * @brief yielded by vehicle on the attention area */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief only collision is detected */ struct NonOccludedCollisionStop @@ -61,10 +67,10 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief occlusion is detected so ego needs to stop at the default stop line position */ struct FirstWaitBeforeOcclusion @@ -76,7 +82,6 @@ struct FirstWaitBeforeOcclusion }; /** - * @struct * @brief ego is approaching the boundary of attention area in the presence of traffic light */ struct PeekingTowardOcclusion @@ -96,7 +101,6 @@ struct PeekingTowardOcclusion }; /** - * @struct * @brief both collision and occlusion are detected in the presence of traffic light */ struct OccludedCollisionStop @@ -110,10 +114,10 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; + std::string safety_report; }; /** - * @struct * @brief at least occlusion is detected in the absence of traffic light */ struct OccludedAbsenceTrafficLight @@ -124,10 +128,10 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; + std::string safety_report; }; /** - * @struct * @brief both collision and occlusion are not detected */ struct Safe @@ -138,7 +142,6 @@ struct Safe }; /** - * @struct * @brief traffic light is red or arrow signal */ struct FullyPrioritized @@ -147,10 +150,12 @@ struct FullyPrioritized size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line + InternalError, //! internal process error, or over the pass judge line + OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle NonOccludedCollisionStop, //! detected collision while FOV is clear @@ -162,41 +167,7 @@ using DecisionResult = std::variant< FullyPrioritized //! only detect vehicles violating traffic rules >; -inline std::string formatDecisionResult(const DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} +std::string formatDecisionResult(const DecisionResult & decision_result); } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c47f016fbdbda..9002c88354d68 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -27,7 +27,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 11deae6bdc001..9624d375de122 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -31,7 +31,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionLanelets */ struct IntersectionLanelets @@ -174,7 +173,6 @@ struct IntersectionLanelets }; /** - * @struct * @brief see the document for more details of PathLanelets */ struct PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 64d20b81e3fad..99d79d4468b38 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -21,7 +21,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionStopLines */ struct IntersectionStopLines @@ -63,9 +62,9 @@ struct IntersectionStopLines /** * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line + * second_attention_lane is null, it is null */ - size_t second_pass_judge_line{0}; + std::optional second_pass_judge_line{std::nullopt}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 615991bc5e027..8ab67c810a30e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -133,6 +133,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + ip.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point = getOrDeclareParameter( + node, + ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp new file mode 100644 index 0000000000000..ea5d89fe8052d --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -0,0 +1,309 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "object_manager.hpp" + +#include +#include +#include // for toPolygon2d + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} + +} // namespace + +namespace behavior_velocity_planner::intersection +{ +namespace bg = boost::geometry; + +ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_str(::to_string(uuid)) +{ +} + +void ObjectInfo::initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + std::optional attention_lanelet_opt_, + std::optional stopline_opt_) +{ + predicted_object_ = object; + attention_lanelet_opt = attention_lanelet_opt_; + stopline_opt = stopline_opt_; + unsafe_interval_ = std::nullopt; + calc_dist_to_stopline(); +} + +void ObjectInfo::update_safety( + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control) +{ + unsafe_interval_ = unsafe_interval; + safe_interval_ = safe_interval; + safe_under_traffic_control_ = safe_under_traffic_control; +} + +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + +void ObjectInfo::calc_dist_to_stopline() +{ + if (!stopline_opt || !attention_lanelet_opt) { + return; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto stopline = stopline_opt.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; + stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; + stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); + dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); +} + +bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + return dist_to_stopline > braking_distance; +} + +bool ObjectInfo::can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const +{ + if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto stopline = stopline_opt.value(); + const auto stopline_p1 = stopline.front(); + const auto stopline_p2 = stopline.back(); + const tier4_autoware_utils::Point2d stopline_mid{ + stopline_p1.x() + stopline_p2.x() / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + const auto attention_lane_end = attention_lanelet.centerline().back(); + const tier4_autoware_utils::LineString2d attention_lane_later_part( + {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; + bg::intersection( + attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); + if (ego_collision_points.empty()) { + return false; + } + const auto expected_collision_point = ego_collision_points.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_ego_path = std::hypot( + expected_collision_point.x() - stopline_mid.x(), + expected_collision_point.y() - stopline_mid.y()); + const double object_to_ego_path = stopline_to_ego_path - stopline_to_object; + // NOTE: if object_to_ego_path < 0, object passed ego path + return object_to_ego_path > tolerable_overshoot; +} + +bool ObjectInfo::before_stopline_by(const double margin) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + return dist_to_stopline < margin; +} + +std::shared_ptr ObjectInfoManager::registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle) +{ + if (objects_info_.count(uuid) == 0) { + auto object = std::make_shared(uuid); + objects_info_[uuid] = object; + } + auto object = objects_info_[uuid]; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } + return object; +} + +void ObjectInfoManager::registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object) +{ + objects_info_[uuid] = object; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } +} + +void ObjectInfoManager::clearObjects() +{ + objects_info_.clear(); + attention_area_objects_.clear(); + intersection_area_objects_.clear(); + parked_objects_.clear(); +}; + +std::vector> ObjectInfoManager::allObjects() +{ + std::vector> all_objects = attention_area_objects_; + all_objects.insert( + all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end()); + all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end()); + return all_objects; +} + +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt) +{ + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (first_itr == predicted_path.path.cend()) { + // even the predicted path end does not collide with the beginning of ego_lane_poly + return std::nullopt; + } + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (last_itr == predicted_path.path.crend()) { + // even the predicted path start does not collide with the end of ego_lane_poly + return std::nullopt; + } + + const size_t enter_idx = static_cast(first_itr - predicted_path.path.begin()); + const double object_enter_time = + static_cast(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; + const double object_exit_time = + static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const auto lane_position = [&]() { + if (first_attention_lane_opt) { + if (lanelet::geometry::inside( + first_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + if (second_attention_lane_opt) { + if (lanelet::geometry::inside( + second_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::SECOND; + } + } + return intersection::CollisionInterval::LanePosition::ELSE; + }(); + + std::vector path; + for (const auto & pose : predicted_path.path) { + path.push_back(pose); + } + return intersection::CollisionInterval{ + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp new file mode 100644 index 0000000000000..e77849570cda8 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -0,0 +1,294 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBJECT_MANAGER_HPP_ +#define OBJECT_MANAGER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + +namespace behavior_velocity_planner::intersection +{ + +/** + * @brief store collision information + */ +struct CollisionInterval +{ + enum LanePosition { + FIRST, + SECOND, + ELSE, + }; + LanePosition lane_position{LanePosition::ELSE}; + + //! original predicted path + std::vector path; + + //! possible collision interval position index on path + std::pair interval_position; + + //! possible collision interval time(without TTC margin) + std::pair interval_time; +}; + +struct CollisionKnowledge +{ + //! the time when the expected collision is judged + rclcpp::Time stamp; + + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; + + //! if !safe, this has value, and it safe, this maybe null if the predicted path does not + //! intersect with ego path + std::optional interval{std::nullopt}; + + double observed_velocity; +}; + +/** + * @brief store collision information of object on the attention area + */ +class ObjectInfo +{ +public: + explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); + + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + { + return predicted_object_; + }; + + std::optional is_unsafe() const + { + if (safe_under_traffic_control_) { + return std::nullopt; + } + if (!unsafe_interval_) { + return std::nullopt; + } + return unsafe_interval_; + } + + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + + /** + * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline + */ + void initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + std::optional attention_lanelet_opt, + std::optional stopline_opt); + + /** + * @brief update unsafe_knowledge + */ + void update_safety( + const std::optional & unsafe_interval_opt, + const std::optional & safe_interval_opt, + const bool safe_under_traffic_control); + + /** + * @brief find the estimated position of the object in the past + */ + std::optional estimated_past_position( + const double past_duration) const; + + /** + * @brief check if object can stop before stopline under the deceleration. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_stopline(const double brake_deceleration) const; + + /** + * @brief check if object can stop before stopline within the overshoot margin. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const; + + /** + * @brief check if the object is before the stopline within the specified margin + */ + bool before_stopline_by(const double margin) const; + + void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_1st_pass_judge_line_passage_ = knowledge; + } + + void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_2nd_pass_judge_line_passage_ = knowledge; + } + + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + + const std::string uuid_str; + +private: + autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + + //! null if the object in intersection_area but not in attention_area + std::optional attention_lanelet_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional stopline_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional dist_to_stopline_opt{std::nullopt}; + + //! store the information if judged as UNSAFE + std::optional unsafe_interval_{std::nullopt}; + + //! store the information if judged as SAFE + std::optional safe_interval_{std::nullopt}; + + //! true if the object is judged as negligible given traffic light color + bool safe_under_traffic_control_{false}; + + std::optional decision_at_1st_pass_judge_line_passage_{std::nullopt}; + std::optional decision_at_2nd_pass_judge_line_passage_{std::nullopt}; + + /** + * @brief calculate/update the distance to corresponding stopline + */ + void calc_dist_to_stopline(); +}; + +/** + * @brief store predicted objects for intersection + */ +class ObjectInfoManager +{ +public: + std::shared_ptr registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked); + + void registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked, + std::shared_ptr object); + + void clearObjects(); + + const std::vector> & attentionObjects() const + { + return attention_area_objects_; + } + + const std::vector> & parkedObjects() const { return parked_objects_; } + + std::vector> allObjects(); + + const std::unordered_map> & + getObjectsMap() + { + return objects_info_; + } + + void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_1st_judge_line_first_time_ = time; + } + void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_2nd_judge_line_first_time_ = time; + } + +private: + std::unordered_map> objects_info_; + + //! belong to attention area + std::vector> attention_area_objects_; + + //! does not belong to attention area but to intersection area + std::vector> intersection_area_objects_; + + //! parked objects on attention_area/intersection_area + std::vector> parked_objects_; + + std::optional passed_1st_judge_line_first_time_{std::nullopt}; + std::optional passed_2nd_judge_line_first_time_{std::nullopt}; +}; + +/** + * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically + */ +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt); + +} // namespace behavior_velocity_planner::intersection + +#endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp index cc6ad880b8153..5d82183cee2fb 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -15,6 +15,7 @@ #ifndef RESULT_HPP_ #define RESULT_HPP_ +#include #include namespace behavior_velocity_planner::intersection @@ -23,10 +24,6 @@ namespace behavior_velocity_planner::intersection template class Result { -public: - static Result make_ok(const Ok & ok) { return Result(ok); } - static Result make_err(const Error & err) { return Result(err); } - public: explicit Result(const Ok & ok) : data_(ok) {} explicit Result(const Error & err) : data_(err) {} @@ -39,6 +36,18 @@ class Result std::variant data_; }; +template +Result make_ok(Args &&... args) +{ + return Result(Ok{std::forward(args)...}); +} + +template +Result make_err(Args &&... args) +{ + return Result(Error{std::forward(args)...}); +} + } // namespace behavior_velocity_planner::intersection #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 90242dc3edd7e..75e2da034780a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -40,6 +40,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, @@ -95,10 +99,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - // set default RTC initializeRTCStatus(); - // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; @@ -112,7 +114,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * reactRTCApproval(decision_result, path, stop_reason); - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -141,133 +142,145 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; - }; - + // ========================================================================================== + // stuck detection + // // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation + // ========================================================================================== const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); if (is_stuck_status) { return is_stuck_status.value(); } + // ========================================================================================== + // basic data validation + // // if attention area is empty, collision/occlusion detection is impossible + // + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line, so ego is already in the middle of the intersection, or the + // end of the ego path has just entered the entry of this intersection + // + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, ego has already passed the intersection, or the end of the + // ego path has just entered the entry of this intersection + // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::Indecisive{"attention area is empty"}; + return intersection::InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::Indecisive{"default stop line is null"}; + return intersection::InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::Indecisive{"occlusion stop line is null"}; + return intersection::InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + // ========================================================================================== + // classify the objects to attention_area/intersection_area and update their position, velocity, + // belonging attention lanelet, distance to corresponding stopline + // ========================================================================================== + updateObjectInfoManagerArea(); + + // ========================================================================================== + // occlusion_status is type of occlusion, + // is_occlusion_cleared_with_margin indicates if occlusion is physically detected + // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for + // intersection_occlusion is disapproved, which means ego is virtually occluded + // + // so is_occlusion_cleared_with_margin should be sent to RTC as module decision + // and is_occlusion_status should be only used to decide ego action + // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is + // not detected but not approved, SAFE is not sent. + // ========================================================================================== + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus(traffic_prioritized_level, interpolated_path_info); - // get intersection area - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const auto is_yield_stuck_status = - isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); - if (is_yield_stuck_status) { - return is_yield_stuck_status.value(); + const auto + [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line] = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + + // ========================================================================================== + // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the + // exit of intersection + // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = + calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); + + // ========================================================================================== + // run collision checking for each objects considering traffic light level. Also if ego just + // passed each pass judge line for the first time, save current collision status for late + // diagnosis + // ========================================================================================== + updateObjectInfoManagerCollision( + path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line); + + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = + generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + if (is_permanent_go_) { + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{safety_diag, evasive_diag}; + } + return intersection::OverPassJudge{ + "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = - getOcclusionStatus( - traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); - - // TODO(Mamoru Sobue): this should be called later for safety diagnosis - const auto is_over_pass_judge_lines_status = - isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); - if (is_over_pass_judge_lines_status) { - return is_over_pass_judge_lines_status.ok(); + const bool collision_on_1st_attention_lane = + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== + std::string safety_report = safety_diag; + if ( + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && + is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } - [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = - is_over_pass_judge_lines_status.err(); - const auto & current_pose = planner_data_->current_odometry->pose; - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto closest_idx = intersection_stoplines.closest_idx; + const bool is_over_default_stopline = util::isOverTargetIndex( + *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx); const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( - *path, collision_stopline_idx, intersection_stoplines, target_objects); + // ========================================================================================== + // pseudo collision detection on green light + // ========================================================================================== + const auto is_green_pseudo_collision_status = + isGreenPseudoCollisionStatus(closest_idx, collision_stopline_idx, intersection_stoplines); if (is_green_pseudo_collision_status) { return is_green_pseudo_collision_status.value(); } - // if ego is waiting for collision detection, the entry time into the intersection is a bit - // delayed for the chattering hold - const bool is_go_out = (activated_ && occlusion_activated_); - const double time_to_restart = - (is_go_out || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, - &ego_ttc_time_array); + // ========================================================================================== + // yield stuck detection + // ========================================================================================== + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); + if (is_yield_stuck_status) { + auto yield_stuck = is_yield_stuck_status.value(); + yield_stuck.safety_report = safety_report; + return yield_stuck; + } - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -276,7 +289,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (is_prioritized) { return intersection::FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, + safety_report}; } // Safe @@ -286,18 +300,51 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; const bool stopped_at_default_line = stoppedForDuration( default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { + // ========================================================================================== // if specified the parameter occlusion.temporal_stop_before_attention_area OR // has_no_traffic_light_, ego will temporarily stop before entering attention area + // ========================================================================================== const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -307,7 +354,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::Indecisive{ + return intersection::InternalError{ "already passed maximum peeking line in the absence of traffic light"}; } return intersection::OccludedAbsenceTrafficLight{ @@ -316,10 +363,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; + occlusion_wo_tl_pass_judge_line_idx, + safety_report}; } + + // ========================================================================================== // following remaining block is "has_traffic_light_" + // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + // ========================================================================================== const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = stoppedAtPosition( @@ -355,7 +407,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + safety_report}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -398,7 +451,17 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::Indecisive & result, + [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + return; +} + +template <> +void prepareRTCByDecisionResult( + [[maybe_unused]] const intersection::OverPassJudge & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -606,7 +669,22 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::Indecisive & decision_result, + [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + [[maybe_unused]] const double baselink2front, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason, + [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] IntersectionModule::DebugData * debug_data) +{ + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + [[maybe_unused]] const bool rtc_default_approved, + [[maybe_unused]] const bool rtc_occlusion_approved, + [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, @@ -640,7 +718,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -689,7 +767,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -807,7 +885,6 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required @@ -1125,84 +1202,7 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -TargetObjects IntersectionModule::generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -intersection::Result< - intersection::Indecisive, - std::pair> -IntersectionModule::isOverPassJudgeLinesStatus( +IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { @@ -1216,8 +1216,11 @@ IntersectionModule::isOverPassJudgeLinesStatus( const size_t pass_judge_line_idx = [&]() { if (planner_param_.occlusion.enable) { if (has_traffic_light_) { + // ========================================================================================== // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its - // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + // position is changed to occlusion_stopline_idx. even if occlusion is cleared by peeking, + // its position should be occlusion_stopline_idx as before + // ========================================================================================== if (passed_1st_judge_line_while_peeking_) { return occlusion_stopline_idx; } @@ -1226,16 +1229,22 @@ IntersectionModule::isOverPassJudgeLinesStatus( if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; - } else { - return first_pass_judge_line_idx; } + // ========================================================================================== + // Otherwise it is first_pass_judge_line + // ========================================================================================== + return first_pass_judge_line_idx; } else if (is_occlusion_state) { + // ========================================================================================== // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area + // ========================================================================================== return occlusion_wo_tl_pass_judge_line_idx; } else { + // ========================================================================================== // if there is no traffic light and occlusion is not detected, pass_judge position is - // default + // default position + // ========================================================================================== return first_pass_judge_line_idx; } } @@ -1246,147 +1255,67 @@ IntersectionModule::isOverPassJudgeLinesStatus( const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); - if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + const auto second_pass_judge_line_idx_opt = intersection_stoplines.second_pass_judge_line; + const std::optional is_over_2nd_pass_judge_line = + second_pass_judge_line_idx_opt + ? std::make_optional(util::isOverTargetIndex( + path, closest_idx, current_pose, second_pass_judge_line_idx_opt.value())) + : std::nullopt; + bool safely_passed_2nd_judge_line_first_time = false; + if ( + is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && + !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_2nd_judge_line_first_time = true; + } + if (second_pass_judge_line_idx_opt) { + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx_opt.value(), baselink2front, path); } - debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); const bool is_over_default_stopline = util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + + const bool over_default_stopline_for_pass_judge = + is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline; + const bool over_pass_judge_line_overall = + is_over_2nd_pass_judge_line ? is_over_2nd_pass_judge_line.value() : is_over_1st_pass_judge_line; if ( - ((is_over_default_stopline || - planner_param_.common.enable_pass_judge_before_default_stopline) && - is_over_2nd_pass_judge_line && was_safe) || + (over_default_stopline_for_pass_judge && over_pass_judge_line_overall && was_safe) || is_permanent_go_) { - /* - * This body is active if ego is - * - over the default stopline AND - * - over the 1st && 2nd pass judge line AND - * - previously safe - * , - * which means ego can stop even if it is over the 1st pass judge line but - * - before default stopline OR - * - before the 2nd pass judge line OR - * - or previously unsafe - * . - * In order for ego to continue peeking or collision detection when occlusion is detected after - * ego passed the 1st pass judge line, it needs to be - * - before the default stopline OR - * - before the 2nd pass judge line OR - * - previously unsafe - */ + // ========================================================================================== + // this body is active if ego is + // - over the default stopline AND + // - over the 1st && 2nd pass judge line AND + // - previously safe + // , + // which means ego can stop even if it is over the 1st pass judge line but + // - before default stopline OR + // - before the 2nd pass judge line OR + // - or previously unsafe + // . + // + // in order for ego to continue peeking or collision detection when occlusion is detected + // after ego passed the 1st pass judge line, it needs to be + // - before the default stopline OR + // - before the 2nd pass judge line OR + // - previously unsafe + // ========================================================================================== is_permanent_go_ = true; - return intersection::Result>::make_ok( - intersection::Indecisive{"over the pass judge line. no plan needed"}); - } - return intersection::Result>::make_err( - std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return { + is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, + safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -/* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel) - { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // consider vehicle in ego-lane && in front of ego - const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive - const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); - - std::vector center_points; - for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); - // get the nearest centerpoint to object - std::vector dist_obj_center_points; - for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, - p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); - // find two center_points whose distances from `closest_centerpoint` cross stopping_distance - double acc_dist_prev = 0.0, acc_dist = 0.0; - auto p1 = center_points[obj_closest_centerpoint_idx]; - auto p2 = center_points[obj_closest_centerpoint_idx]; - for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } - } - // if stopping_distance >= center_points, stopping_point is center_points[end] - const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); - // linear interpolation - geometry_msgs::msg::Point stopping_point; - stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); - stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio); - stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio); - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point); - stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0); - stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); - - // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; - predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; - predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); - const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); - debug_data_.predicted_obj_pose.position = stopping_point; - debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - - if (is_in_stuck_area) { - return true; - } - return false; - } -*/ - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8a34aabe8b918..9527ce8e5ebea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -19,6 +19,7 @@ #include "interpolated_path_info.hpp" #include "intersection_lanelets.hpp" #include "intersection_stoplines.hpp" +#include "object_manager.hpp" #include "result.hpp" #include @@ -45,32 +46,6 @@ namespace behavior_velocity_planner { -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -/** - * @struct - * @brief categorize TargetObjects - */ -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -154,6 +129,10 @@ class IntersectionModule : public SceneModuleInterface { double object_margin_to_path; } ignore_on_red_traffic_light; + struct AvoidCollisionByAcceleration + { + double object_time_margin_to_collision_point; + } avoid_collision_by_acceleration; } collision_detection; struct Occlusion @@ -204,36 +183,37 @@ class IntersectionModule : public SceneModuleInterface std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional first_pass_judge_wall_pose{std::nullopt}; + std::optional second_pass_judge_wall_pose{std::nullopt}; bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional first_attention_area{std::nullopt}; std::optional second_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; using TimeDistanceArray = std::vector>; /** - * @struct * @brief categorize traffic light priority */ enum class TrafficPrioritizedLevel { @@ -246,6 +226,41 @@ class IntersectionModule : public SceneModuleInterface }; /** @} */ + /** + * @brief + */ + struct PassJudgeStatus + { + //! true if ego is over the 1st pass judge line + const bool is_over_1st_pass_judge; + + //! true if second_attention_lane exists and ego is over the 2nd pass judge line + const std::optional is_over_2nd_pass_judge; + + //! true only when ego passed 1st pass judge line safely for the first time + const bool safely_passed_1st_judge_line; + + //! true only when ego passed 2nd pass judge line safely for the first time + const bool safely_passed_2nd_judge_line; + }; + + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -325,6 +340,9 @@ class IntersectionModule : public SceneModuleInterface //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; + + //! save the time when ego observed green traffic light before entering the intersection + std::optional initial_green_light_observed_time_{std::nullopt}; /** @}*/ private: @@ -340,17 +358,19 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -363,6 +383,9 @@ class IntersectionModule : public SceneModuleInterface */ //! debouncing for stable SAFE decision StateMachine collision_state_machine_; + + //! container for storing safety status of objects on the attention area + intersection::ObjectInfoManager object_info_manager_; /** @} */ private: @@ -388,8 +411,6 @@ class IntersectionModule : public SceneModuleInterface StateMachine static_occlusion_timeout_state_machine_; /** @} */ - std::optional initial_green_light_observed_time_{std::nullopt}; - private: /** *********************************************************** @@ -464,13 +485,13 @@ class IntersectionModule : public SceneModuleInterface /** * @brief prepare basic data structure - * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @return return IntersectionStopLines if all data is valid, otherwise InternalError * @note if successful, it is ensure that intersection_lanelets_, * intersection_lanelets.first_conflicting_lane are not null * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( + intersection::Result prepareIntersectionData( const bool is_prioritized, PathWithLaneId * path); /** @@ -518,16 +539,6 @@ class IntersectionModule : public SceneModuleInterface const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); /** @} */ -private: - /** - * @defgroup utility [fn] utility member function - * @{ - */ - void stoppedAtPositionForDuration( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, - const double duration, StateMachine * state_machine); - /** @} */ - private: /** *********************************************************** @@ -547,14 +558,6 @@ class IntersectionModule : public SceneModuleInterface TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; /** @} */ -private: - /** - * @brief categorize target objects - */ - TargetObjects generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const; - private: /** *********************************************************** @@ -598,14 +601,12 @@ class IntersectionModule : public SceneModuleInterface std::optional isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const; + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -621,27 +622,21 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check occlusion status * @attention this function has access to value() of occlusion_attention_divisions_, - * intersection_lanelets.first_attention_area() + * intersection_lanelets_ intersection_lanelets.first_attention_area() */ std::tuple< OcclusionType, bool /* module detection with margin */, bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects); + const intersection::InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); + OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); /** @} */ private: @@ -654,15 +649,12 @@ class IntersectionModule : public SceneModuleInterface */ /** * @brief check if ego is already over the pass judge line - * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return * (is_over_1st_pass_judge, is_over_2nd_pass_judge) * @attention this function has access to value() of intersection_stoplines.default_stopline, * intersection_stoplines.occlusion_stopline */ - intersection::Result< - intersection::Indecisive, - std::pair> - isOverPassJudgeLinesStatus( + PassJudgeStatus isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -678,6 +670,25 @@ class IntersectionModule : public SceneModuleInterface bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** + * @brief find the objects on attention_area/intersection_area and update positional information + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerArea(); + + /** + * @brief find the collision Interval/CollisionKnowledge of registered objects + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + + void cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + /** * @brief check if there are any objects around the stoplines on the attention areas when ego * entered the intersection on green light @@ -687,44 +698,58 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_peeking_stopline */ std::optional isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects); + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines); + + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateDetectionBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; /** - * @brief check collision + * @brief generate the message explaining how much ego should accelerate to avoid future dangerous + * situation */ - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level); + std::string generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const TimeDistanceArray & ego_time_distance_array, + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + + /** + * @brief return if collision is detected and the collision position + */ + CollisionStatus detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const bool is_parked_vehicle) const; - void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); - /** * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline, + * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); /** @} */ - /* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel); - */ - mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 43bb68cf56f67..4f7e8b45d107f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,42 +18,18 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include -#include #include +#include +#include +#include #include -namespace -{ -tier4_autoware_utils::Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - tier4_autoware_utils::Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - tier4_autoware_utils::Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -79,38 +55,303 @@ bool IntersectionModule::isTargetCollisionVehicleType( return false; } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) +void IntersectionModule::updateObjectInfoManagerArea() { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(); - if (!is_green_solid_on) { - return std::nullopt; + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + // ========================================================================================== + // entries that are not observed in this iteration need to be cleared + // + // NOTE: old_map is not referenced because internal data of object_info_manager is cleared + // ========================================================================================== + const auto old_map = object_info_manager_.getObjectsMap(); + object_info_manager_.clearObjects(); + + for (const auto & predicted_object : planner_data_->predicted_objects->objects) { + if (!isTargetCollisionVehicleType(predicted_object)) { + continue; + } + + // ========================================================================================== + // NOTE: is_parked_vehicle is used because sometimes slow vehicle direction is + // incorrect/reversed/flipped due to tracking. if is_parked_vehicle is true, object direction + // is not checked + // ========================================================================================== + const auto object_direction = + util::getObjectPoseWithVelocityDirection(predicted_object.kinematics); + const auto is_parked_vehicle = + std::fabs(predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; + } + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const bool in_intersection_area = [&]() { + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + if (!belong_attention_lanelet_id && !in_intersection_area) { + continue; + } else if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } + + const auto object_it = old_map.find(predicted_object.object_id); + if (object_it != old_map.end()) { + auto object_info = object_it->second; + object_info_manager_.registerExistingObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle, object_info); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } else { + auto object_info = object_info_manager_.registerObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } } - const auto closest_idx = intersection_stoplines.closest_idx; - const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path.points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); +} + +void IntersectionModule::updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, + const IntersectionModule::TimeDistanceArray & time_distance_array, + const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) +{ + const auto & intersection_lanelets = intersection_lanelets_.value(); + + if (passed_1st_judge_line_first_time) { + object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now()); + } + if (passed_2nd_judge_line_first_time) { + object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now()); + } + + const double passing_time = time_distance_array.back().first; + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = + lanelet::utils::getArcCoordinates(concat_lanelets, planner_data_->current_odometry->pose); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + + for (auto & object_info : object_info_manager_.attentionObjects()) { + const auto & predicted_object = object_info->predicted_object(); + bool safe_under_traffic_control = false; + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + object_info->can_stop_before_stopline( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)) { + safe_under_traffic_control = true; + } + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + object_info->can_stop_before_ego_lane( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration, + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path, + ego_lane)) { + safe_under_traffic_control = true; + } + + // ========================================================================================== + // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe + // CollisionKnowledge for most probable path + // ========================================================================================== + std::list sorted_predicted_paths; + for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { + sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); + } + sorted_predicted_paths.sort( + [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; }); + + // ========================================================================================== + // if all of the predicted path is lower confidence/geometrically does not intersect with ego + // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either + // of them has value, not both + // ========================================================================================== + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; + + for (const auto & predicted_path_ptr : sorted_predicted_paths) { + auto predicted_path = *predicted_path_ptr; + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + continue; + } + cutPredictPathWithinDuration( + planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); + const auto object_passage_interval_opt = intersection::findPassageInterval( + predicted_path, predicted_object.shape, ego_poly, + intersection_lanelets.first_attention_lane(), + intersection_lanelets.second_attention_lane()); + if (!object_passage_interval_opt) { + // there is no chance of geometric collision for the entire prediction horizon + continue; + } + const auto & object_passage_interval = object_passage_interval_opt.value(); + const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto ego_start_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_start_itr == time_distance_array.end()) { + // ========================================================================================== + // this is the case where at time "object_enter_time - collision_start_margin_time", ego is + // arriving at the exit of the intersection, which means even if we assume that the object + // accelerates and the first collision happens faster by the TTC margin, ego will be already + // arriving at the exist of the intersection. + // ========================================================================================== + continue; + } + auto ego_end_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_end_itr == time_distance_array.end()) { + ego_end_itr = time_distance_array.end() - 1; + } + const double ego_start_arc_length = std::max( + 0.0, closest_arc_coords.length + ego_start_itr->second - + planner_data_->vehicle_info_.rear_overhang_m); + const double ego_end_arc_length = std::min( + closest_arc_coords.length + ego_end_itr->second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + const auto trimmed_ego_polygon = lanelet::utils::getPolygonFromArcLength( + concat_lanelets, ego_start_arc_length, ego_end_arc_length); + if (trimmed_ego_polygon.empty()) { + continue; + } + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + const auto & object_path = object_passage_interval.path; + const auto [begin, end] = object_passage_interval.interval_position; + bool collision_detected = false; + for (auto i = begin; i <= end; ++i) { + if (bg::intersects( + polygon, + tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + collision_detected = true; + break; + } + } + if (collision_detected) { + // if judged as UNSAFE, return + safe_interval = std::nullopt; + unsafe_interval = object_passage_interval; + break; + } + if (!safe_interval) { + // ========================================================================================== + // save the safe_decision_knowledge for the most probable path. this value is nullified if + // judged UNSAFE during the iteration + // ========================================================================================== + safe_interval = object_passage_interval; + } + } + object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); + if (passed_1st_judge_line_first_time) { + object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + if (passed_2nd_judge_line_first_time) { + object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + } +} + +void IntersectionModule::cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * path) const +{ + const rclcpp::Time current_time = clock_->now(); + const auto original_path = path->path; + path->path.clear(); + + for (size_t k = 0; k < original_path.size(); ++k) { // each path points + const auto & predicted_pose = original_path.at(k); + const auto predicted_time = + rclcpp::Time(object_stamp) + rclcpp::Duration(path->time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + path->path.push_back(predicted_pose); } } +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + // ========================================================================================== + // if there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because collision is likely to happen. + // ========================================================================================== if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool still_wait = @@ -119,22 +360,341 @@ IntersectionModule::isGreenPseudoCollisionStatus( if (!still_wait) { return std::nullopt; } + const auto & attention_objects = object_info_manager_.attentionObjects(); const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; + attention_objects.begin(), attention_objects.end(), [&](const auto & object_info) { + return object_info->before_stopline_by( + planner_param_.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline); }); if (exist_close_vehicles) { + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } return std::nullopt; } +std::string IntersectionModule::generateDetectionBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: format library causes runtime fault if the # of placeholders is more than the # of + // vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the first attention lanelet of type {7} with the estimated " + "current velocity {8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the lanelet of type {7} with the estimated current velocity " + "{8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const IntersectionModule::TimeDistanceArray & ego_time_distance_array, + const std::vector>> & + too_late_detect_objects, + [[maybe_unused]] const std::vector>> & + misjudge_objects) const +{ + static constexpr double min_vel = 1e-2; + std::string diag; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if (!object_info->unsafe_interval()) { + continue; + } + // object side + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const auto [begin, end] = unsafe_interval.interval_position; + const auto &p1 = unsafe_interval.path.at(begin).position, + p2 = unsafe_interval.path.at(end).position; + const auto collision_pos = + tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto object_dist_to_margin_point = + tier4_autoware_utils::calcDistance2d( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, + collision_pos) - + planner_param_.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point * + object_info->observed_velocity(); + if (object_dist_to_margin_point <= 0.0) { + continue; + } + const auto object_eta_to_margin_point = + object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); + // ego side + const double ego_dist_to_collision_pos = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + const auto ego_eta_to_collision_pos_it = std::lower_bound( + ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, + [](const auto & a, const double b) { return a.second < b; }); + if (ego_eta_to_collision_pos_it == ego_time_distance_array.end()) { + continue; + } + const double ego_eta_to_collision_pos = ego_eta_to_collision_pos_it->first; + if (ego_eta_to_collision_pos < object_eta_to_margin_point) { + // this case, ego will pass the collision point before this object get close to the collision + // point within margin just by keeping current plan, so no need to accelerate + continue; + } + diag += fmt::format( + "the object is expected to approach the collision point by the TTC margin in {0} seconds, " + "while ego will arrive there in {1} seconds, so ego needs to accelerate from current " + "velocity {2}[m/s] to {3}[m/s]\n", + object_eta_to_margin_point, // 0 + ego_eta_to_collision_pos, // 1 + ego_vel, // 2 + ego_dist_to_collision_pos / object_eta_to_margin_point // 3 + ); + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) +{ + // ========================================================================================== + // if collision is detected for multiple objects, we prioritize collision on the first + // attention lanelet + // ========================================================================================== + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + // ========================================================================================== + // if ego is over the pass judge lines, then the visualization as "too_late_objects" or + // "misjudge_objects" is more important than that for "unsafe" + // + // NOTE: consider a vehicle which was not detected at 1st_pass_judge_passage, and now collision + // detected on the 1st lane, which is "too_late" for 1st lane passage, but once it decelerated + // or yielded, so it turned safe, and ego passed the 2nd pass judge line, but at the same it + // accelerated again, which is "misjudge" for 2nd lane passage. In this case this vehicle is + // visualized as "misjudge" + // ========================================================================================== + auto * debug_container = &debug_data_.unsafe_targets.objects; + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if ( + is_over_1st_pass_judge_line && + unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + if (is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value()) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); + if ( + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + debug_container->emplace_back(object_info->predicted_object()); + } + if (collision_at_first_lane) { + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + } + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; +} + +/* bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, const intersection::PathLanelets & path_lanelets, const size_t closest_idx, @@ -383,6 +943,7 @@ bool IntersectionModule::checkCollision( return collision_detected; } +*/ std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -422,32 +983,9 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( return std::nullopt; } -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { const double intersection_velocity = @@ -460,8 +998,42 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; const double current_velocity = planner_data_->current_velocity->twist.linear.x; - int assigned_lane_found = false; + // ========================================================================================== + // if ego is waiting for collision detection, the entry time into the intersection + // is a bit delayed for the chattering hold, so we need to "shift" the TimeDistanceArray by + // this delay + // ========================================================================================== + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_delay = (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + // ========================================================================================== + // to account for the stopline generated by upstream behavior_velocity modules (like walkway, + // crosswalk), if use_upstream flag is true, the raw velocity of path points after + // last_intersection_stopline_candidate_idx is used, which maybe almost-zero. at those almost-zero + // velocity path points, ego future profile is almost "fixed" there. + // + // last_intersection_stopline_candidate_idx must be carefully decided especially when ego + // velocity is almost zero, because if last_intersection_stopline_candidate_idx is at the + // closest_idx for example, ego is almost "fixed" at current position for the entire + // spatiotemporal profile, which is judged as SAFE because that profile does not collide + // with the predicted paths of objects. + // + // if second_attention_lane exists, second_attention_stopline_idx is used. if not, + // max(occlusion_stopline_idx, first_attention_stopline_idx) is used because + // occlusion_stopline_idx varies depending on the peeking offset parameter + // ========================================================================================== + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() + : std::max(occlusion_stopline_idx, first_attention_stopline_idx); + + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 353826070eff7..b7c2d4c12878c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -34,25 +34,18 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); // check occlusion on detection lane - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? detectOcclusion( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) + ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -76,13 +69,14 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & attention_areas = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + const auto & lane_divisions = occlusion_attention_divisions_.value(); + const auto & occ_grid = *planner_data_->occupancy_grid; const auto & current_pose = planner_data_->current_odometry->pose; const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; @@ -208,13 +202,15 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + debug_data_.parked_targets.objects.push_back( + blocking_attention_object_info->predicted_object()); } std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + const Polygon2d obj_poly = + tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -390,14 +386,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( LineString2d ego_occlusion_line; ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + for (const auto & attention_object_info : object_info_manager_.allObjects()) { + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { return OcclusionType::DYNAMICALLY_OCCLUDED; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 746e615d8c6b0..c44008db9b08b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -161,11 +161,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -intersection::Result +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + +Result IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) { - using intersection::Result; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -177,26 +179,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return Result::make_err( - intersection::Indecisive{"splineInterpolate failed"}); + return make_err( + "splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return Result::make_err( - intersection::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + return make_err( + "Path has no interval on intersection lane " + std::to_string(lane_id_)); } - // cache intersection lane information because it is invariant if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); } auto & intersection_lanelets = intersection_lanelets_.value(); - - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // ========================================================================================== + // at the very first time of registration of this module, the path may not be conflicting with + // the attention area, so update() is called to update the internal data as well as traffic + // light info + // ========================================================================================== intersection_lanelets.update( is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); @@ -205,17 +213,17 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return Result::make_err( - intersection::Indecisive{"conflicting area is empty"}); + return make_err( + "conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes + // ========================================================================================== + // even if the attention area is null, stuck vehicle stop line needs to be generated from + // conflicting lanes, so dummy_first_attention_lane is used + // ========================================================================================== const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value() : first_conflicting_lane; @@ -224,8 +232,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return Result::make_err( - intersection::Indecisive{"failed to generate intersection_stoplines"}); + return make_err( + "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto closest_idx = intersection_stoplines.closest_idx; @@ -239,8 +247,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return Result::make_err( - intersection::Indecisive{"failed to generate PathLanelets"}); + return make_err( + "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -250,8 +258,24 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - return Result::make_ok( - BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); + const bool is_green_solid_on = isGreenSolidOn(); + if (is_green_solid_on && !initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + + return make_ok( + interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( @@ -421,8 +445,10 @@ IntersectionModule::generateIntersectionStopLines( int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { + // ========================================================================================== // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. + // ========================================================================================== const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stopline_idx_ip_opt) { @@ -457,12 +483,13 @@ IntersectionModule::generateIntersectionStopLines( second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; + // (8) second pass judge line position on interpolated path. It is null if second_attention_lane + // is null + size_t second_pass_judge_line_ip = occlusion_wo_tl_pass_judge_line_ip; + bool second_pass_judge_line_valid = false; + if (second_attention_area_opt) { + second_pass_judge_line_valid = true; + } struct IntersectionStopLinesTemp { @@ -528,9 +555,11 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } + if (second_pass_judge_line_valid) { + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + } intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -556,7 +585,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // for low priority lane - // If ego_lane has right of way (i.e. is high priority), + // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) lanelet::ConstLanelets yield_lanelets{}; const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7f23bed3c36cd..389c73d651f1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -262,21 +262,19 @@ bool IntersectionModule::checkStuckVehicleInIntersection( std::optional IntersectionModule::isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const + const intersection::IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; - const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK - const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK - const auto first_attention_stopline_idx = - intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + interpolated_path_info, intersection_lanelets.attention_non_preceding()); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -297,14 +295,13 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { @@ -358,19 +355,20 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { + for (const auto & object_info : object_info_manager_.attentionObjects()) { + const auto & object = object_info->predicted_object(); const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); if (obj_v_norm > stuck_vehicle_vel_thr) { continue; } for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); if (is_in_lanelet) { - debug_data_.yield_stuck_targets.objects.push_back(object.object); + debug_data_.yield_stuck_targets.objects.push_back(object); return true; } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f103191b2dc6f..e37bb3ee88cc1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -34,7 +34,6 @@ namespace behavior_velocity_planner::util { /** - * @fn * @brief insert a new pose to the path and return its index * @return if insertion was successful return the inserted point index */ @@ -44,7 +43,6 @@ std::optional insertPointIndex( const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** - * @fn * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( @@ -52,7 +50,6 @@ bool hasLaneIds( const std::set & ids); /** - * @fn * @brief find the first contiguous interval of the path points that contains the specified lane ids * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is * found, returns the pair (start-1, end) @@ -61,7 +58,6 @@ std::optional> findLaneIdsInterval( const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @fn * @brief return the index of the first point which is inside the given polygon * @param[in] lane_interval the interval of the path points on the intersection * @param[in] search_forward flag for search direction @@ -72,7 +68,6 @@ std::optional getFirstPointInsidePolygon( const bool search_forward = true); /** - * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -84,7 +79,6 @@ bool isOverTargetIndex( const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** - * @fn * @brief check if ego is before the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -99,13 +93,11 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** - * @fn * @brief check if the given lane has related traffic light */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** - * @fn * @brief interpolate PathWithLaneId */ std::optional generateInterpolatedPath( @@ -117,7 +109,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** - * @fn * @brief this function sorts the set of lanelets topologically using topological sort and merges * the lanelets from each root to each end. each branch is merged and returned with the original * lanelets @@ -131,7 +122,6 @@ mergeLaneletsByTopologicalSort( const lanelet::routing::RoutingGraphPtr routing_graph_ptr); /** - * @fn * @brief find the index of the first point where vehicle footprint intersects with the given * polygon */ @@ -140,6 +130,10 @@ std::optional getFirstPointInsidePolygonByFootprint( const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ std::optional> getFirstPointInsidePolygonsByFootprint(