41
41
#include < string>
42
42
#include < tuple>
43
43
#include < utility>
44
+ #include < variant>
44
45
#include < vector>
45
46
46
47
namespace behavior_velocity_planner
@@ -170,16 +171,25 @@ class IntersectionModule : public SceneModuleInterface
170
171
} debug;
171
172
};
172
173
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 };
182
188
};
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>;
183
193
184
194
struct DebugData
185
195
{
@@ -301,11 +311,9 @@ class IntersectionModule : public SceneModuleInterface
301
311
bool getOcclusionSafety () const { return occlusion_safety_; }
302
312
double getOcclusionDistance () const { return occlusion_stop_distance_; }
303
313
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_; }
305
315
306
316
private:
307
- rclcpp::Node & node_;
308
-
309
317
/* *
310
318
***********************************************************
311
319
***********************************************************
@@ -417,7 +425,7 @@ class IntersectionModule : public SceneModuleInterface
417
425
* @defgroup occlusion-variables [var] occlusion detection variables
418
426
* @{
419
427
*/
420
- OcclusionType prev_occlusion_status_;
428
+ OcclusionType prev_occlusion_status_{NotOccluded{}} ;
421
429
422
430
// ! debouncing for the first brief stop at the default stopline
423
431
StateMachine before_creep_state_machine_;
@@ -520,7 +528,7 @@ class IntersectionModule : public SceneModuleInterface
520
528
*/
521
529
std::optional<size_t > getStopLineIndexFromMap (
522
530
const intersection::InterpolatedPathInfo & interpolated_path_info,
523
- lanelet::ConstLanelet assigned_lanelet);
531
+ lanelet::ConstLanelet assigned_lanelet) const ;
524
532
525
533
/* *
526
534
* @brief generate IntersectionStopLines
@@ -531,15 +539,15 @@ class IntersectionModule : public SceneModuleInterface
531
539
const lanelet::ConstLanelet & first_attention_lane,
532
540
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
533
541
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 ;
535
543
536
544
/* *
537
545
* @brief generate IntersectionLanelets
538
546
*/
539
547
intersection::IntersectionLanelets generateObjectiveLanelets (
540
548
lanelet::LaneletMapConstPtr lanelet_map_ptr,
541
549
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
542
- const lanelet::ConstLanelet assigned_lanelet);
550
+ const lanelet::ConstLanelet assigned_lanelet) const ;
543
551
544
552
/* *
545
553
* @brief generate PathLanelets
@@ -550,14 +558,15 @@ class IntersectionModule : public SceneModuleInterface
550
558
const lanelet::CompoundPolygon3d & first_conflicting_area,
551
559
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
552
560
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 ;
554
563
555
564
/* *
556
565
* @brief generate discretized detection lane linestring.
557
566
*/
558
567
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions (
559
568
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 ;
561
570
/* * @} */
562
571
563
572
private:
@@ -664,7 +673,8 @@ class IntersectionModule : public SceneModuleInterface
664
673
* @attention this function has access to value() of intersection_lanelets_,
665
674
* intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
666
675
*/
667
- OcclusionType detectOcclusion (const intersection::InterpolatedPathInfo & interpolated_path_info);
676
+ OcclusionType detectOcclusion (
677
+ const intersection::InterpolatedPathInfo & interpolated_path_info) const ;
668
678
/* * @} */
669
679
670
680
private:
@@ -727,7 +737,7 @@ class IntersectionModule : public SceneModuleInterface
727
737
*/
728
738
std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus (
729
739
const size_t closest_idx, const size_t collision_stopline_idx,
730
- const intersection::IntersectionStopLines & intersection_stoplines);
740
+ const intersection::IntersectionStopLines & intersection_stoplines) const ;
731
741
732
742
/* *
733
743
* @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
@@ -759,7 +769,8 @@ class IntersectionModule : public SceneModuleInterface
759
769
* @brief return if collision is detected and the collision position
760
770
*/
761
771
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 ;
763
774
764
775
std::optional<size_t > checkAngleForTargetLanelets (
765
776
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
@@ -775,7 +786,7 @@ class IntersectionModule : public SceneModuleInterface
775
786
TimeDistanceArray calcIntersectionPassingTime (
776
787
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
777
788
const intersection::IntersectionStopLines & intersection_stoplines,
778
- tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
789
+ tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const ;
779
790
/* * @} */
780
791
781
792
mutable DebugData debug_data_;
0 commit comments