Skip to content

Commit 01ea64a

Browse files
committed
refactor(intersection): add const
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 0b90a97 commit 01ea64a

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
@@ -166,16 +167,25 @@ class IntersectionModule : public SceneModuleInterface
166167
} debug;
167168
};
168169

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

180190
struct DebugData
181191
{
@@ -297,11 +307,9 @@ class IntersectionModule : public SceneModuleInterface
297307
bool getOcclusionSafety() const { return occlusion_safety_; }
298308
double getOcclusionDistance() const { return occlusion_stop_distance_; }
299309
void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; }
300-
bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; }
310+
bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; }
301311

302312
private:
303-
rclcpp::Node & node_;
304-
305313
/**
306314
***********************************************************
307315
***********************************************************
@@ -413,7 +421,7 @@ class IntersectionModule : public SceneModuleInterface
413421
* @defgroup occlusion-variables [var] occlusion detection variables
414422
* @{
415423
*/
416-
OcclusionType prev_occlusion_status_;
424+
OcclusionType prev_occlusion_status_{NotOccluded{}};
417425

418426
//! debouncing for the first brief stop at the default stopline
419427
StateMachine before_creep_state_machine_;
@@ -516,7 +524,7 @@ class IntersectionModule : public SceneModuleInterface
516524
*/
517525
std::optional<size_t> getStopLineIndexFromMap(
518526
const intersection::InterpolatedPathInfo & interpolated_path_info,
519-
lanelet::ConstLanelet assigned_lanelet);
527+
lanelet::ConstLanelet assigned_lanelet) const;
520528

521529
/**
522530
* @brief generate IntersectionStopLines
@@ -527,15 +535,15 @@ class IntersectionModule : public SceneModuleInterface
527535
const lanelet::ConstLanelet & first_attention_lane,
528536
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
529537
const intersection::InterpolatedPathInfo & interpolated_path_info,
530-
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
538+
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const;
531539

532540
/**
533541
* @brief generate IntersectionLanelets
534542
*/
535543
intersection::IntersectionLanelets generateObjectiveLanelets(
536544
lanelet::LaneletMapConstPtr lanelet_map_ptr,
537545
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
538-
const lanelet::ConstLanelet assigned_lanelet);
546+
const lanelet::ConstLanelet assigned_lanelet) const;
539547

540548
/**
541549
* @brief generate PathLanelets
@@ -546,14 +554,15 @@ class IntersectionModule : public SceneModuleInterface
546554
const lanelet::CompoundPolygon3d & first_conflicting_area,
547555
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
548556
const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,
549-
const std::vector<lanelet::CompoundPolygon3d> & attention_areas, const size_t closest_idx);
557+
const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
558+
const size_t closest_idx) const;
550559

551560
/**
552561
* @brief generate discretized detection lane linestring.
553562
*/
554563
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
555564
lanelet::ConstLanelets detection_lanelets,
556-
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution);
565+
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
557566
/** @} */
558567

559568
private:
@@ -660,7 +669,8 @@ class IntersectionModule : public SceneModuleInterface
660669
* @attention this function has access to value() of intersection_lanelets_,
661670
* intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
662671
*/
663-
OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info);
672+
OcclusionType detectOcclusion(
673+
const intersection::InterpolatedPathInfo & interpolated_path_info) const;
664674
/** @} */
665675

666676
private:
@@ -723,7 +733,7 @@ class IntersectionModule : public SceneModuleInterface
723733
*/
724734
std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus(
725735
const size_t closest_idx, const size_t collision_stopline_idx,
726-
const intersection::IntersectionStopLines & intersection_stoplines);
736+
const intersection::IntersectionStopLines & intersection_stoplines) const;
727737

728738
/**
729739
* @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
@@ -755,7 +765,8 @@ class IntersectionModule : public SceneModuleInterface
755765
* @brief return if collision is detected and the collision position
756766
*/
757767
CollisionStatus detectCollision(
758-
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line);
768+
const bool is_over_1st_pass_judge_line,
769+
const std::optional<bool> is_over_2nd_pass_judge_line) const;
759770

760771
std::optional<size_t> checkAngleForTargetLanelets(
761772
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
@@ -771,7 +782,7 @@ class IntersectionModule : public SceneModuleInterface
771782
TimeDistanceArray calcIntersectionPassingTime(
772783
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
773784
const intersection::IntersectionStopLines & intersection_stoplines,
774-
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
785+
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
775786
/** @} */
776787

777788
mutable DebugData debug_data_;

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

+15-13
Original file line numberDiff line numberDiff line change
@@ -346,7 +346,7 @@ void IntersectionModule::cutPredictPathWithinDuration(
346346
std::optional<intersection::NonOccludedCollisionStop>
347347
IntersectionModule::isGreenPseudoCollisionStatus(
348348
const size_t closest_idx, const size_t collision_stopline_idx,
349-
const intersection::IntersectionStopLines & intersection_stoplines)
349+
const intersection::IntersectionStopLines & intersection_stoplines) const
350350
{
351351
// ==========================================================================================
352352
// if there are any vehicles on the attention area when ego entered the intersection on green
@@ -588,7 +588,8 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis(
588588
}
589589

590590
IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
591-
const bool is_over_1st_pass_judge_line, const std::optional<bool> is_over_2nd_pass_judge_line)
591+
const bool is_over_1st_pass_judge_line,
592+
const std::optional<bool> is_over_2nd_pass_judge_line) const
592593
{
593594
// ==========================================================================================
594595
// if collision is detected for multiple objects, we prioritize collision on the first
@@ -598,14 +599,18 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
598599
bool collision_at_non_first_lane = false;
599600

600601
// ==========================================================================================
601-
// find the objects which is judges as UNSAFE after ego passed pass judge lines.
602+
// find the objects which are judged as UNSAFE after ego passed pass judge lines.
602603
//
603604
// misjudge_objects are those that were once judged as safe when ego passed the pass judge line
604605
//
605-
// too_late_detect objects are those that (1) were not detected when ego passed the pass judge
606-
// line (2) were judged as dangerous at the same time when ego passed the pass judge, which are
607-
// expected to have been detected in the prior iteration because ego could have judged as UNSAFE
608-
// in the prior iteration
606+
// too_late_detect_objects are those that (1) were not detected when ego passed the pass judge
607+
// line (2) were judged as dangerous at the same time when ego passed the pass judge line, which
608+
// means they were expected to have been detected when ego passed the pass judge lines or in the
609+
// prior iteration, because ego could have judged them as UNSAFE if their information was
610+
// available at that time.
611+
//
612+
// that case is both "too late to stop" and "too late to go" for the planner. and basically
613+
// detection side is responsible for this fault
609614
// ==========================================================================================
610615
std::vector<std::pair<CollisionStatus::BlameType, std::shared_ptr<intersection::ObjectInfo>>>
611616
misjudge_objects;
@@ -617,13 +622,10 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
617622
object_info->predicted_object());
618623
continue;
619624
}
620-
if (!object_info->is_unsafe()) {
625+
if (!object_info->unsafe_info()) {
621626
continue;
622627
}
623-
const auto & unsafe_info = object_info->is_unsafe().value();
624-
setObjectsOfInterestData(
625-
object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
626-
object_info->predicted_object().shape, ColorName::RED);
628+
const auto & unsafe_info = object_info->unsafe_info().value();
627629
// ==========================================================================================
628630
// if ego is over the pass judge lines, then the visualization as "too_late_objects" or
629631
// "misjudge_objects" is more important than that for "unsafe"
@@ -989,7 +991,7 @@ std::optional<size_t> IntersectionModule::checkAngleForTargetLanelets(
989991
IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
990992
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
991993
const intersection::IntersectionStopLines & intersection_stoplines,
992-
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array)
994+
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const
993995
{
994996
const double intersection_velocity =
995997
planner_param_.collision_detection.velocity_profile.default_velocity;

0 commit comments

Comments
 (0)