From 01ea64a2cdb7acc8af3aca0b9094f51063d2a988 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Feb 2024 03:34:49 +0900 Subject: [PATCH] refactor(intersection): add const 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 | 28 +++++----- .../src/scene_intersection_occlusion.cpp | 19 ++++--- .../src/scene_intersection_prepare_data.cpp | 10 ++-- 7 files changed, 91 insertions(+), 68 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 293103b078450..a6204ff218353 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -52,7 +52,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), @@ -88,10 +87,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); } @@ -105,11 +104,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); @@ -225,6 +226,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); @@ -241,17 +250,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."; @@ -375,7 +384,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_diag}; + safety_report}; } // ========================================================================================== @@ -383,7 +392,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 752c21158ac95..d4775f6dac852 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 { @@ -297,11 +307,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_; - /** *********************************************************** *********************************************************** @@ -413,7 +421,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_; @@ -516,7 +524,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 @@ -527,7 +535,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 @@ -535,7 +543,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 @@ -546,14 +554,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: @@ -660,7 +669,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: @@ -723,7 +733,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 @@ -755,7 +765,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, @@ -771,7 +782,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 4496df77134e2..4b3915c8de572 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,13 +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(); - setObjectsOfInterestData( - object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, - object_info->predicted_object().shape, ColorName::RED); + 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" @@ -989,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 b9fedaceb1fed..cc3c46f2718e8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -62,9 +62,10 @@ IntersectionModule::getOcclusionStatus( (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red_or_no_tl_info_ever) ? 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 @@ -72,11 +73,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; @@ -85,7 +86,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(); @@ -103,7 +104,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 = @@ -394,7 +395,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 = @@ -406,9 +407,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 5f62d10e60387..1ffdb75204e4f 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 @@ -288,7 +288,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(); @@ -344,7 +344,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; @@ -575,7 +575,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 = @@ -780,7 +780,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; @@ -848,7 +848,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;