From f7dd320c2452fb549d0e18234bcfe33107433f8c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 7 Feb 2024 17:22:58 +0900 Subject: [PATCH] chore(intersection): add const to member functions, replace enum with class tag for occlusion (#6299) Signed-off-by: Mamoru Sobue --- .../src/object_manager.cpp | 2 +- .../src/object_manager.hpp | 4 +- .../src/scene_intersection.cpp | 41 ++++++++------ .../src/scene_intersection.hpp | 55 +++++++++++-------- .../src/scene_intersection_collision.cpp | 25 +++++---- .../src/scene_intersection_occlusion.cpp | 19 ++++--- .../src/scene_intersection_prepare_data.cpp | 10 ++-- 7 files changed, 91 insertions(+), 65 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index fe327704f61c2..420031e4df1cf 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects() parked_objects_.clear(); }; -std::vector> ObjectInfoManager::allObjects() +std::vector> ObjectInfoManager::allObjects() const { std::vector> all_objects = attention_area_objects_; all_objects.insert( diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index e77849570cda8..77e39637523a9 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -109,7 +109,7 @@ class ObjectInfo return predicted_object_; }; - std::optional is_unsafe() const + std::optional unsafe_info() const { if (safe_under_traffic_control_) { return std::nullopt; @@ -246,7 +246,7 @@ class ObjectInfoManager const std::vector> & parkedObjects() const { return parked_objects_; } - std::vector> allObjects(); + std::vector> allObjects() const; const std::unordered_map> & getObjectsMap() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3cf243b7893fc..dc9c70a2ebc04 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -51,7 +51,6 @@ IntersectionModule::IntersectionModule( const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), @@ -87,10 +86,10 @@ IntersectionModule::IntersectionModule( } decision_state_pub_ = - node_.create_publisher("~/debug/intersection/decision_state", 1); - ego_ttc_pub_ = node_.create_publisher( + node.create_publisher("~/debug/intersection/decision_state", 1); + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node_.create_publisher( + object_ttc_pub_ = node.create_publisher( "~/debug/intersection/object_ttc", 1); } @@ -104,11 +103,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; - const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + - intersection::formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); + { + const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + + intersection::formatDecisionResult(decision_result); + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + } prepareRTCStatus(decision_result, *path); @@ -224,6 +225,14 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line); + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (!object_info->unsafe_info()) { + continue; + } + setObjectsOfInterestData( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, + object_info->predicted_object().shape, ColorName::RED); + } 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); @@ -240,17 +249,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( "no collision is detected", "ego can safely pass the intersection at this rate"}; } - 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) { + if (const bool collision_on_1st_attention_lane = + has_collision && + (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() && + !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."; @@ -374,7 +383,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_diag}; + safety_report}; } // ========================================================================================== @@ -382,7 +391,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // // 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_static_occlusion = std::holds_alternative(occlusion_status); const bool is_stuck_by_static_occlusion = stoppedAtPosition( occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9527ce8e5ebea..e62a58d63f923 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -166,16 +167,25 @@ class IntersectionModule : public SceneModuleInterface } debug; }; - enum OcclusionType { - //! occlusion is not detected - NOT_OCCLUDED, - //! occlusion is not caused by dynamic objects - STATICALLY_OCCLUDED, - //! occlusion is caused by dynamic objects - DYNAMICALLY_OCCLUDED, - //! actual occlusion does not exist, only disapproved by RTC - RTC_OCCLUDED, + //! occlusion is not detected + struct NotOccluded + { + double best_clearance_distance{-1.0}; + }; + //! occlusion is not caused by dynamic objects + struct StaticallyOccluded + { + double best_clearance_distance{0.0}; + }; + //! occlusion is caused by dynamic objects + struct DynamicallyOccluded + { + double best_clearance_distance{0.0}; }; + //! actual occlusion does not exist, only disapproved by RTC + using RTCOccluded = std::monostate; + using OcclusionType = + std::variant; struct DebugData { @@ -294,11 +304,9 @@ class IntersectionModule : public SceneModuleInterface bool getOcclusionSafety() const { return occlusion_safety_; } double getOcclusionDistance() const { return occlusion_stop_distance_; } void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } - bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; } + bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; } private: - rclcpp::Node & node_; - /** *********************************************************** *********************************************************** @@ -396,7 +404,7 @@ class IntersectionModule : public SceneModuleInterface * @defgroup occlusion-variables [var] occlusion detection variables * @{ */ - OcclusionType prev_occlusion_status_; + OcclusionType prev_occlusion_status_{NotOccluded{}}; //! debouncing for the first brief stop at the default stopline StateMachine before_creep_state_machine_; @@ -499,7 +507,7 @@ class IntersectionModule : public SceneModuleInterface */ std::optional getStopLineIndexFromMap( const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet); + lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate IntersectionStopLines @@ -510,7 +518,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -518,7 +526,7 @@ class IntersectionModule : public SceneModuleInterface intersection::IntersectionLanelets generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet); + const lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate PathLanelets @@ -529,14 +537,15 @@ class IntersectionModule : public SceneModuleInterface const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx); + const std::vector & attention_areas, + const size_t closest_idx) const; /** * @brief generate discretized detection lane linestring. */ std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const; /** @} */ private: @@ -636,7 +645,8 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); + OcclusionType detectOcclusion( + const intersection::InterpolatedPathInfo & interpolated_path_info) const; /** @} */ private: @@ -699,7 +709,7 @@ class IntersectionModule : public SceneModuleInterface */ std::optional isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines); + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and @@ -731,7 +741,8 @@ class IntersectionModule : public SceneModuleInterface * @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); + const bool is_over_1st_pass_judge_line, + const std::optional is_over_2nd_pass_judge_line) const; std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -747,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( 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); + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ mutable DebugData debug_data_; 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 4f7e8b45d107f..d7eba93166cfc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -346,7 +346,7 @@ void IntersectionModule::cutPredictPathWithinDuration( std::optional IntersectionModule::isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) + const intersection::IntersectionStopLines & intersection_stoplines) const { // ========================================================================================== // if there are any vehicles on the attention area when ego entered the intersection on green @@ -588,7 +588,8 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( } IntersectionModule::CollisionStatus IntersectionModule::detectCollision( - const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) + const bool is_over_1st_pass_judge_line, + const std::optional is_over_2nd_pass_judge_line) const { // ========================================================================================== // if collision is detected for multiple objects, we prioritize collision on the first @@ -598,14 +599,18 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( bool collision_at_non_first_lane = false; // ========================================================================================== - // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // find the objects which are judged 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 + // 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 line, which + // means they were expected to have been detected when ego passed the pass judge lines or in the + // prior iteration, because ego could have judged them as UNSAFE if their information was + // available at that time. + // + // that case is both "too late to stop" and "too late to go" for the planner. and basically + // detection side is responsible for this fault // ========================================================================================== std::vector>> misjudge_objects; @@ -617,10 +622,10 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( object_info->predicted_object()); continue; } - if (!object_info->is_unsafe()) { + if (!object_info->unsafe_info()) { continue; } - const auto & unsafe_info = object_info->is_unsafe().value(); + const auto & unsafe_info = object_info->unsafe_info().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" @@ -986,7 +991,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( 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) + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; 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 b7c2d4c12878c..364e585f10757 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -46,9 +46,10 @@ IntersectionModule::getOcclusionStatus( auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? detectOcclusion(interpolated_path_info) - : OcclusionType::NOT_OCCLUDED; + : NotOccluded{}; occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + std::holds_alternative(occlusion_status) ? StateMachine::State::GO + : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection @@ -56,11 +57,11 @@ IntersectionModule::getOcclusionStatus( const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); // RTC's detection if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; + occlusion_status = RTCOccluded{}; } const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // including approval - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + if (is_occlusion_state && std::holds_alternative(occlusion_status)) { occlusion_status = prev_occlusion_status_; } else { prev_occlusion_status_ = occlusion_status; @@ -69,7 +70,7 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) + const intersection::InterpolatedPathInfo & interpolated_path_info) const { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); @@ -87,7 +88,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return OcclusionType::NOT_OCCLUDED; + return NotOccluded{}; } const auto first_inside_attention_idx_ip_opt = @@ -378,7 +379,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return OcclusionType::NOT_OCCLUDED; + return NotOccluded{min_dist}; } debug_data_.nearest_occlusion_projection = @@ -390,9 +391,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( 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; + return DynamicallyOccluded{min_dist}; } } - return OcclusionType::STATICALLY_OCCLUDED; + return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner 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 c44008db9b08b..8125bc05e43f0 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 @@ -280,7 +280,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL std::optional IntersectionModule::getStopLineIndexFromMap( const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet) + lanelet::ConstLanelet assigned_lanelet) const { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -336,7 +336,7 @@ IntersectionModule::generateIntersectionStopLines( const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; @@ -567,7 +567,7 @@ IntersectionModule::generateIntersectionStopLines( intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet) + const lanelet::ConstLanelet assigned_lanelet) const { const double detection_area_length = planner_param_.common.attention_area_length; const double occlusion_detection_area_length = @@ -772,7 +772,7 @@ std::optional IntersectionModule::generatePathLanele const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx) + const std::vector & attention_areas, const size_t closest_idx) const { const double width = planner_data_->vehicle_info_.vehicle_width_m; static constexpr double path_lanelet_interval = 1.5; @@ -840,7 +840,7 @@ std::optional IntersectionModule::generatePathLanele std::vector IntersectionModule::generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const { const double curvature_threshold = planner_param_.occlusion.attention_lane_crop_curvature_threshold;