Skip to content

Commit 3a10282

Browse files
authored
chore(intersection): add const to member functions, replace enum with class tag for occlusion (#6299)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent b574523 commit 3a10282

7 files changed

+91
-68
lines changed

planning/behavior_velocity_intersection_module/src/object_manager.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects()
240240
parked_objects_.clear();
241241
};
242242

243-
std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects()
243+
std::vector<std::shared_ptr<ObjectInfo>> ObjectInfoManager::allObjects() const
244244
{
245245
std::vector<std::shared_ptr<ObjectInfo>> all_objects = attention_area_objects_;
246246
all_objects.insert(

planning/behavior_velocity_intersection_module/src/object_manager.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ class ObjectInfo
109109
return predicted_object_;
110110
};
111111

112-
std::optional<CollisionInterval> is_unsafe() const
112+
std::optional<CollisionInterval> unsafe_info() const
113113
{
114114
if (safe_under_traffic_control_) {
115115
return std::nullopt;
@@ -246,7 +246,7 @@ class ObjectInfoManager
246246

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

249-
std::vector<std::shared_ptr<ObjectInfo>> allObjects();
249+
std::vector<std::shared_ptr<ObjectInfo>> allObjects() const;
250250

251251
const std::unordered_map<unique_identifier_msgs::msg::UUID, std::shared_ptr<ObjectInfo>> &
252252
getObjectsMap()

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+25-16
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,6 @@ IntersectionModule::IntersectionModule(
5252
const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node,
5353
const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
5454
: SceneModuleInterface(module_id, logger, clock),
55-
node_(node),
5655
lane_id_(lane_id),
5756
associative_ids_(associative_ids),
5857
turn_direction_(turn_direction),
@@ -88,10 +87,10 @@ IntersectionModule::IntersectionModule(
8887
}
8988

9089
decision_state_pub_ =
91-
node_.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
92-
ego_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
90+
node.create_publisher<std_msgs::msg::String>("~/debug/intersection/decision_state", 1);
91+
ego_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
9392
"~/debug/intersection/ego_ttc", 1);
94-
object_ttc_pub_ = node_.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
93+
object_ttc_pub_ = node.create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>(
9594
"~/debug/intersection/object_ttc", 1);
9695
}
9796

@@ -105,11 +104,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
105104
const auto decision_result = modifyPathVelocityDetail(path, stop_reason);
106105
prev_decision_result_ = decision_result;
107106

108-
const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
109-
intersection::formatDecisionResult(decision_result);
110-
std_msgs::msg::String decision_result_msg;
111-
decision_result_msg.data = decision_type;
112-
decision_state_pub_->publish(decision_result_msg);
107+
{
108+
const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " +
109+
intersection::formatDecisionResult(decision_result);
110+
std_msgs::msg::String decision_result_msg;
111+
decision_result_msg.data = decision_type;
112+
decision_state_pub_->publish(decision_result_msg);
113+
}
113114

114115
prepareRTCStatus(decision_result, *path);
115116

@@ -225,6 +226,14 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
225226
updateObjectInfoManagerCollision(
226227
path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line,
227228
safely_passed_2nd_judge_line);
229+
for (const auto & object_info : object_info_manager_.attentionObjects()) {
230+
if (!object_info->unsafe_info()) {
231+
continue;
232+
}
233+
setObjectsOfInterestData(
234+
object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
235+
object_info->predicted_object().shape, ColorName::RED);
236+
}
228237

229238
const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] =
230239
detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line);
@@ -241,17 +250,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
241250
"no collision is detected", "ego can safely pass the intersection at this rate"};
242251
}
243252

244-
const bool collision_on_1st_attention_lane =
245-
has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST);
246253
// ==========================================================================================
247254
// this state is very dangerous because ego is very close/over the boundary of 1st attention lane
248255
// and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this
249256
// case, possible another collision may be expected on the 2nd attention lane too.
250257
// ==========================================================================================
251258
std::string safety_report = safety_diag;
252-
if (
253-
is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line &&
254-
is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
259+
if (const bool collision_on_1st_attention_lane =
260+
has_collision &&
261+
(collision_position == intersection::CollisionInterval::LanePosition::FIRST);
262+
is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() &&
263+
!is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) {
255264
safety_report +=
256265
"\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st "
257266
"attention lane, which is dangerous.";
@@ -375,15 +384,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
375384
closest_idx,
376385
first_attention_stopline_idx,
377386
occlusion_wo_tl_pass_judge_line_idx,
378-
safety_diag};
387+
safety_report};
379388
}
380389

381390
// ==========================================================================================
382391
// following remaining block is "has_traffic_light_"
383392
//
384393
// if ego is stuck by static occlusion in the presence of traffic light, start timeout count
385394
// ==========================================================================================
386-
const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED;
395+
const bool is_static_occlusion = std::holds_alternative<StaticallyOccluded>(occlusion_status);
387396
const bool is_stuck_by_static_occlusion =
388397
stoppedAtPosition(
389398
occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) &&

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

+33-22
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141
#include <string>
4242
#include <tuple>
4343
#include <utility>
44+
#include <variant>
4445
#include <vector>
4546

4647
namespace behavior_velocity_planner
@@ -170,16 +171,25 @@ class IntersectionModule : public SceneModuleInterface
170171
} debug;
171172
};
172173

173-
enum OcclusionType {
174-
//! occlusion is not detected
175-
NOT_OCCLUDED,
176-
//! occlusion is not caused by dynamic objects
177-
STATICALLY_OCCLUDED,
178-
//! occlusion is caused by dynamic objects
179-
DYNAMICALLY_OCCLUDED,
180-
//! actual occlusion does not exist, only disapproved by RTC
181-
RTC_OCCLUDED,
174+
//! occlusion is not detected
175+
struct NotOccluded
176+
{
177+
double best_clearance_distance{-1.0};
178+
};
179+
//! occlusion is not caused by dynamic objects
180+
struct StaticallyOccluded
181+
{
182+
double best_clearance_distance{0.0};
183+
};
184+
//! occlusion is caused by dynamic objects
185+
struct DynamicallyOccluded
186+
{
187+
double best_clearance_distance{0.0};
182188
};
189+
//! actual occlusion does not exist, only disapproved by RTC
190+
using RTCOccluded = std::monostate;
191+
using OcclusionType =
192+
std::variant<NotOccluded, StaticallyOccluded, DynamicallyOccluded, RTCOccluded>;
183193

184194
struct DebugData
185195
{
@@ -301,11 +311,9 @@ class IntersectionModule : public SceneModuleInterface
301311
bool getOcclusionSafety() const { return occlusion_safety_; }
302312
double getOcclusionDistance() const { return occlusion_stop_distance_; }
303313
void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
304-
bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }
314+
bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }
305315

306316
private:
307-
rclcpp::Node & node_;
308-
309317
/**
310318
***********************************************************
311319
***********************************************************
@@ -417,7 +425,7 @@ class IntersectionModule : public SceneModuleInterface
417425
* @defgroup occlusion-variables [var] occlusion detection variables
418426
* @{
419427
*/
420-
OcclusionType prev_occlusion_status_;
428+
OcclusionType prev_occlusion_status_{NotOccluded{}};
421429

422430
//! debouncing for the first brief stop at the default stopline
423431
StateMachine before_creep_state_machine_;
@@ -520,7 +528,7 @@ class IntersectionModule : public SceneModuleInterface
520528
*/
521529
std::optional<size_t> getStopLineIndexFromMap(
522530
const intersection::InterpolatedPathInfo & interpolated_path_info,
523-
lanelet::ConstLanelet assigned_lanelet);
531+
lanelet::ConstLanelet assigned_lanelet) const;
524532

525533
/**
526534
* @brief generate IntersectionStopLines
@@ -531,15 +539,15 @@ class IntersectionModule : public SceneModuleInterface
531539
const lanelet::ConstLanelet & first_attention_lane,
532540
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
533541
const intersection::InterpolatedPathInfo & interpolated_path_info,
534-
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
542+
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const;
535543

536544
/**
537545
* @brief generate IntersectionLanelets
538546
*/
539547
intersection::IntersectionLanelets generateObjectiveLanelets(
540548
lanelet::LaneletMapConstPtr lanelet_map_ptr,
541549
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
542-
const lanelet::ConstLanelet assigned_lanelet);
550+
const lanelet::ConstLanelet assigned_lanelet) const;
543551

544552
/**
545553
* @brief generate PathLanelets
@@ -550,14 +558,15 @@ class IntersectionModule : public SceneModuleInterface
550558
const lanelet::CompoundPolygon3d & first_conflicting_area,
551559
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
552560
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
553-
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
561+
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
562+
const size_t closest_idx) const;
554563

555564
/**
556565
* @brief generate discretized detection lane linestring.
557566
*/
558567
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
559568
lanelet::ConstLanelets detection_lanelets,
560-
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
569+
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
561570
/** @} */
562571

563572
private:
@@ -664,7 +673,8 @@ class IntersectionModule : public SceneModuleInterface
664673
* @attention this function has access to value() of intersection_lanelets_,
665674
* intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
666675
*/
667-
OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info);
676+
OcclusionType detectOcclusion(
677+
const intersection::InterpolatedPathInfo & interpolated_path_info) const;
668678
/** @} */
669679

670680
private:
@@ -727,7 +737,7 @@ class IntersectionModule : public SceneModuleInterface
727737
*/
728738
std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus(
729739
const size_t closest_idx, const size_t collision_stopline_idx,
730-
const intersection::IntersectionStopLines & intersection_stoplines);
740+
const intersection::IntersectionStopLines & intersection_stoplines) const;
731741

732742
/**
733743
* @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
@@ -759,7 +769,8 @@ class IntersectionModule : public SceneModuleInterface
759769
* @brief return if collision is detected and the collision position
760770
*/
761771
CollisionStatus detectCollision(
762-
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line);
772+
const bool is_over_1st_pass_judge_line,
773+
const std::optional<bool> is_over_2nd_pass_judge_line) const;
763774

764775
std::optional<size_t> checkAngleForTargetLanelets(
765776
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
@@ -775,7 +786,7 @@ class IntersectionModule : public SceneModuleInterface
775786
TimeDistanceArray calcIntersectionPassingTime(
776787
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
777788
const intersection::IntersectionStopLines & intersection_stoplines,
778-
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
789+
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
779790
/** @} */
780791

781792
mutable DebugData debug_data_;

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

+15-13
Original file line numberDiff line numberDiff line change
@@ -351,7 +351,7 @@ void IntersectionModule::cutPredictPathWithinDuration(
351351
std::optional<intersection::NonOccludedCollisionStop>
352352
IntersectionModule::isGreenPseudoCollisionStatus(
353353
const size_t closest_idx, const size_t collision_stopline_idx,
354-
const intersection::IntersectionStopLines & intersection_stoplines)
354+
const intersection::IntersectionStopLines & intersection_stoplines) const
355355
{
356356
// ==========================================================================================
357357
// if there are any vehicles on the attention area when ego entered the intersection on green
@@ -593,7 +593,8 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis(
593593
}
594594

595595
IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
596-
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line)
596+
const bool is_over_1st_pass_judge_line,
597+
const std::optional<bool> is_over_2nd_pass_judge_line) const
597598
{
598599
// ==========================================================================================
599600
// if collision is detected for multiple objects, we prioritize collision on the first
@@ -603,14 +604,18 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
603604
bool collision_at_non_first_lane = false;
604605

605606
// ==========================================================================================
606-
// find the objects which is judges as UNSAFE after ego passed pass judge lines.
607+
// find the objects which are judged as UNSAFE after ego passed pass judge lines.
607608
//
608609
// misjudge_objects are those that were once judged as safe when ego passed the pass judge line
609610
//
610-
// too_late_detect objects are those that (1) were not detected when ego passed the pass judge
611-
// line (2) were judged as dangerous at the same time when ego passed the pass judge, which are
612-
// expected to have been detected in the prior iteration because ego could have judged as UNSAFE
613-
// in the prior iteration
611+
// too_late_detect_objects are those that (1) were not detected when ego passed the pass judge
612+
// line (2) were judged as dangerous at the same time when ego passed the pass judge line, which
613+
// means they were expected to have been detected when ego passed the pass judge lines or in the
614+
// prior iteration, because ego could have judged them as UNSAFE if their information was
615+
// available at that time.
616+
//
617+
// that case is both "too late to stop" and "too late to go" for the planner. and basically
618+
// detection side is responsible for this fault
614619
// ==========================================================================================
615620
std::vector<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
616621
misjudge_objects;
@@ -622,13 +627,10 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
622627
object_info->predicted_object());
623628
continue;
624629
}
625-
if (!object_info->is_unsafe()) {
630+
if (!object_info->unsafe_info()) {
626631
continue;
627632
}
628-
const auto & unsafe_info = object_info->is_unsafe().value();
629-
setObjectsOfInterestData(
630-
object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
631-
object_info->predicted_object().shape, ColorName::RED);
633+
const auto & unsafe_info = object_info->unsafe_info().value();
632634
// ==========================================================================================
633635
// if ego is over the pass judge lines, then the visualization as "too_late_objects" or
634636
// "misjudge_objects" is more important than that for "unsafe"
@@ -994,7 +996,7 @@ std::optional<size_t> IntersectionModule::checkAngleForTargetLanelets(
994996
IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
995997
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
996998
const intersection::IntersectionStopLines & intersection_stoplines,
997-
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array)
999+
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const
9981000
{
9991001
const double intersection_velocity =
10001002
planner_param_.collision_detection.velocity_profile.default_velocity;

0 commit comments

Comments
 (0)