Skip to content

Commit

Permalink
chore(intersection): add const to member functions, replace enum with…
Browse files Browse the repository at this point in the history 
… class tag for occlusion (autowarefoundation#6299)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Oct 23, 2024
1 parent 8c57547 commit f7dd320
Show file tree
Hide file tree
Showing 7 changed files with 91 additions and 65 deletions.
2 changes: 1 addition & 1 deletion planning/behavior_velocity_intersection_module/src/object_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects()
parked_objects_.clear();
};

std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects()
std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
{
std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
all_objects.insert(
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_velocity_intersection_module/src/object_manager.hpp
View file
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
41 changes: 25 additions & 16 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -87,10 +86,10 @@ IntersectionModule::IntersectionModule(
}

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 @@ -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);

Expand Down Expand Up @@ -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);
Expand All @@ -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.";
Expand Down Expand Up @@ -374,15 +383,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
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);
const bool is_stuck_by_static_occlusion =
stoppedAtPosition(
occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) &&
Expand Down
55 changes: 33 additions & 22 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
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 @@ -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_;

/**
***********************************************************
***********************************************************
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -499,7 +507,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 @@ -510,15 +518,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 @@ -529,14 +537,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 @@ -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:
Expand Down Expand Up @@ -699,7 +709,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 @@ -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<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 @@ -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_;
Expand Down
25 changes: 15 additions & 10 deletions planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,7 +346,7 @@ void IntersectionModule::cutPredictPathWithinDuration(
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 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis(
}

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 @@ 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<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
misjudge_objects;
Expand All @@ -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"
Expand Down Expand Up @@ -986,7 +991,7 @@ std::optional<size_t> 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;
Expand Down
Loading

0 comments on commit f7dd320

Please sign in to comment.