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
@@ -166,16 +167,25 @@ class IntersectionModule : public SceneModuleInterface
166
167
} debug;
167
168
};
168
169
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 };
178
184
};
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>;
179
189
180
190
struct DebugData
181
191
{
@@ -297,11 +307,9 @@ class IntersectionModule : public SceneModuleInterface
297
307
bool getOcclusionSafety () const { return occlusion_safety_; }
298
308
double getOcclusionDistance () const { return occlusion_stop_distance_; }
299
309
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_; }
301
311
302
312
private:
303
- rclcpp::Node & node_;
304
-
305
313
/* *
306
314
***********************************************************
307
315
***********************************************************
@@ -413,7 +421,7 @@ class IntersectionModule : public SceneModuleInterface
413
421
* @defgroup occlusion-variables [var] occlusion detection variables
414
422
* @{
415
423
*/
416
- OcclusionType prev_occlusion_status_;
424
+ OcclusionType prev_occlusion_status_{NotOccluded{}} ;
417
425
418
426
// ! debouncing for the first brief stop at the default stopline
419
427
StateMachine before_creep_state_machine_;
@@ -516,7 +524,7 @@ class IntersectionModule : public SceneModuleInterface
516
524
*/
517
525
std::optional<size_t > getStopLineIndexFromMap (
518
526
const intersection::InterpolatedPathInfo & interpolated_path_info,
519
- lanelet::ConstLanelet assigned_lanelet);
527
+ lanelet::ConstLanelet assigned_lanelet) const ;
520
528
521
529
/* *
522
530
* @brief generate IntersectionStopLines
@@ -527,15 +535,15 @@ class IntersectionModule : public SceneModuleInterface
527
535
const lanelet::ConstLanelet & first_attention_lane,
528
536
const std::optional<lanelet::CompoundPolygon3d> & second_attention_area_opt,
529
537
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 ;
531
539
532
540
/* *
533
541
* @brief generate IntersectionLanelets
534
542
*/
535
543
intersection::IntersectionLanelets generateObjectiveLanelets (
536
544
lanelet::LaneletMapConstPtr lanelet_map_ptr,
537
545
lanelet::routing::RoutingGraphPtr routing_graph_ptr,
538
- const lanelet::ConstLanelet assigned_lanelet);
546
+ const lanelet::ConstLanelet assigned_lanelet) const ;
539
547
540
548
/* *
541
549
* @brief generate PathLanelets
@@ -546,14 +554,15 @@ class IntersectionModule : public SceneModuleInterface
546
554
const lanelet::CompoundPolygon3d & first_conflicting_area,
547
555
const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
548
556
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 ;
550
559
551
560
/* *
552
561
* @brief generate discretized detection lane linestring.
553
562
*/
554
563
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions (
555
564
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 ;
557
566
/* * @} */
558
567
559
568
private:
@@ -660,7 +669,8 @@ class IntersectionModule : public SceneModuleInterface
660
669
* @attention this function has access to value() of intersection_lanelets_,
661
670
* intersection_lanelets.first_attention_area(), occlusion_attention_divisions_
662
671
*/
663
- OcclusionType detectOcclusion (const intersection::InterpolatedPathInfo & interpolated_path_info);
672
+ OcclusionType detectOcclusion (
673
+ const intersection::InterpolatedPathInfo & interpolated_path_info) const ;
664
674
/* * @} */
665
675
666
676
private:
@@ -723,7 +733,7 @@ class IntersectionModule : public SceneModuleInterface
723
733
*/
724
734
std::optional<intersection::NonOccludedCollisionStop> isGreenPseudoCollisionStatus (
725
735
const size_t closest_idx, const size_t collision_stopline_idx,
726
- const intersection::IntersectionStopLines & intersection_stoplines);
736
+ const intersection::IntersectionStopLines & intersection_stoplines) const ;
727
737
728
738
/* *
729
739
* @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and
@@ -755,7 +765,8 @@ class IntersectionModule : public SceneModuleInterface
755
765
* @brief return if collision is detected and the collision position
756
766
*/
757
767
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 ;
759
770
760
771
std::optional<size_t > checkAngleForTargetLanelets (
761
772
const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets,
@@ -771,7 +782,7 @@ class IntersectionModule : public SceneModuleInterface
771
782
TimeDistanceArray calcIntersectionPassingTime (
772
783
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
773
784
const intersection::IntersectionStopLines & intersection_stoplines,
774
- tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array);
785
+ tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const ;
775
786
/* * @} */
776
787
777
788
mutable DebugData debug_data_;
0 commit comments