Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore(intersection): add const to member functions, replace enum with class tag for occlusion #6299

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -240,7 +240,7 @@
parked_objects_.clear();
};

std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects()
std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const

Check warning on line 243 in planning/behavior_velocity_intersection_module/src/object_manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/object_manager.cpp#L243

Added line #L243 was not covered by tests
{
std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
all_objects.insert(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ class ObjectInfo
return predicted_object_;
};

std::optional<CollisionInterval> is_unsafe() const
std::optional<CollisionInterval> unsafe_info() const
{
if (safe_under_traffic_control_) {
return std::nullopt;
Expand Down Expand Up @@ -246,7 +246,7 @@ class ObjectInfoManager

const std::vector<std::shared_ptr<ObjectInfo>> & parkedObjects() const { return parked_objects_; }

std::vector<std::shared_ptr<ObjectInfo>> allObjects();
std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;

const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
getObjectsMap()
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1134 to 1143, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity

The mean cyclomatic complexity increases from 4.68 to 4.76, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -52,7 +52,6 @@
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),
Expand Down Expand Up @@ -88,10 +87,10 @@
}

decision_state_pub_ =
node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
ego_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
"~/debug/intersection/ego_ttc", 1);
object_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
"~/debug/intersection/object_ttc", 1);
}

Expand All @@ -105,11 +104,13 @@
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);

Expand Down Expand Up @@ -225,6 +226,14 @@
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;

Check warning on line 231 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp#L231

Added line #L231 was not covered by tests
}
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);
Expand All @@ -241,17 +250,17 @@
"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 =

Check warning on line 259 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp#L259

Added line #L259 was not covered by tests
has_collision &&
(collision_position == intersection::CollisionInterval::LanePosition::FIRST);

Check warning on line 261 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp#L261

Added line #L261 was not covered by tests
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) {

Check notice on line 263 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Conditional

IntersectionModule::modifyPathVelocityDetail no longer has a complex conditional. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
safety_report +=
"\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st "
"attention lane, which is dangerous.";
Expand Down Expand Up @@ -375,15 +384,15 @@
closest_idx,
first_attention_stopline_idx,
occlusion_wo_tl_pass_judge_line_idx,
safety_diag};
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_static_occlusion = std::holds_alternative<StaticallyOccluded>(occlusion_status);

Check notice on line 395 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

IntersectionModule::modifyPathVelocityDetail increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 395 in planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

IntersectionModule::modifyPathVelocityDetail increases in cyclomatic complexity from 45 to 48, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
const bool is_stuck_by_static_occlusion =
stoppedAtPosition(
occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) &&
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <string>
#include <tuple>
#include <utility>
#include <variant>
#include <vector>

namespace behavior_velocity_planner
Expand Down Expand Up @@ -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<NotOccluded, StaticallyOccluded, DynamicallyOccluded, RTCOccluded>;

struct DebugData
{
Expand Down Expand Up @@ -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_;

/**
***********************************************************
***********************************************************
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -516,7 +524,7 @@ class IntersectionModule : public SceneModuleInterface
*/
std::optional<size_t> getStopLineIndexFromMap(
const intersection::InterpolatedPathInfo & interpolated_path_info,
lanelet::ConstLanelet assigned_lanelet);
lanelet::ConstLanelet assigned_lanelet) const;

/**
* @brief generate IntersectionStopLines
Expand All @@ -527,15 +535,15 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::ConstLanelet & first_attention_lane,
const std::optional<lanelet::CompoundPolygon3d> & 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
*/
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
Expand All @@ -546,14 +554,15 @@ class IntersectionModule : public SceneModuleInterface
const lanelet::CompoundPolygon3d & first_conflicting_area,
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
const size_t closest_idx) const;

/**
* @brief generate discretized detection lane linestring.
*/
std::vector<lanelet::ConstLineString3d> 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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -723,7 +733,7 @@ class IntersectionModule : public SceneModuleInterface
*/
std::optional<intersection::NonOccludedCollisionStop> 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
Expand Down Expand Up @@ -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<bool> is_over_2nd_pass_judge_line);
const bool is_over_1st_pass_judge_line,
const std::optional<bool> is_over_2nd_pass_judge_line) const;

std::optional<size_t> checkAngleForTargetLanelets(
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
Expand All @@ -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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -346,7 +346,7 @@
std::optional<intersection::NonOccludedCollisionStop>
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
Expand Down Expand Up @@ -588,7 +588,8 @@
}

IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line)
const bool is_over_1st_pass_judge_line,
const std::optional<bool> is_over_2nd_pass_judge_line) const
{
// ==========================================================================================
// if collision is detected for multiple objects, we prioritize collision on the first
Expand All @@ -598,14 +599,18 @@
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<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
misjudge_objects;
Expand All @@ -617,13 +622,10 @@
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();

Check warning on line 628 in planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp#L628

Added line #L628 was not covered by tests
// ==========================================================================================
// 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"
Expand Down Expand Up @@ -989,7 +991,7 @@
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;
Expand Down
Loading
Loading