From c20f13149e8df27216bf701b01139bba9b424ad8 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 18 Dec 2023 12:09:10 -0500 Subject: [PATCH 1/4] add end time --- .../regulatory_elements/CarmaTrafficSignal.h | 24 +++++++++-------- .../lib/CarmaTrafficSignal.cpp | 26 +++++++++---------- 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index 15741929d17..fd779b34dea 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -25,9 +25,9 @@ namespace lanelet { /** - * @brief: Enum representing Traffic Light States. + * @brief: Enum representing Traffic Light States. * These states match the SAE J2735 PhaseState definitions used for SPaT messages - * + * * UNAVAILABLE : No data available * DARK : Light is non-functional * STOP_THEN_PROCEED : Flashing Red @@ -53,6 +53,8 @@ enum class CarmaTrafficSignalState { CAUTION_CONFLICTING_TRAFFIC=9 }; +const int INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES = 2147483647; + struct CarmaTrafficSignalRoleNameString { static constexpr char ControlStart[] = "control_start"; @@ -67,7 +69,7 @@ std::ostream& operator<<(std::ostream& os, CarmaTrafficSignalState s); /** * @brief: Class representing a known timing traffic light. * Normally the traffic light timing information is provided by SAE J2735 SPaT messages although alternative data sources can be supported - * + * * @ingroup RegulatoryElementPrimitives * @ingroup Primitives */ @@ -79,7 +81,7 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement int revision_ = 0; //indicates when was this last modified boost::posix_time::time_duration fixed_cycle_duration; - std::vector recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps , + std::vector recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps , //used in dynamic SPAT processing std::vector> recorded_time_stamps; std::unordered_map signal_durations; @@ -98,11 +100,11 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement * NOTE: Order of the lanelets does not correlate to the order of the control_end lanelets */ lanelet::ConstLanelets getControlStartLanelets() const; - + /** * @brief getControlEndLanelets function returns lanelets where this element's control ends * NOTE: Order of the lanelets does not correlate to the order of the control_start lanelets - * + * */ lanelet::ConstLanelets getControlEndLanelets() const; @@ -115,7 +117,7 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement * @throw InvalidInputError if timestamps recorded somehow did not have full cycle */ boost::optional> predictState(boost::posix_time::ptime time_stamp); - + /** * @brief Return the stop_lines related to the entry lanelets in order if exists. */ @@ -124,16 +126,16 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement /** * @brief Return the stop_lines related to the specified entry lanelet - * @param llt entry_lanelet - * @return optional stop line linestring. - * Empty optional if no stopline, or no entry lanelets, or if specified entry lanelet is not recorded. + * @param llt entry_lanelet + * @return optional stop line linestring. + * Empty optional if no stopline, or no entry lanelets, or if specified entry lanelet is not recorded. */ Optional getConstStopLine(const ConstLanelet& llt); Optional getStopLine(const ConstLanelet& llt); explicit CarmaTrafficSignal(const lanelet::RegulatoryElementDataPtr& data); /** - * @brief: Creating one is not directly usable unless setStates is called + * @brief: Creating one is not directly usable unless setStates is called * Static helper function that creates a stop line data object based on the provided inputs * * @param id The lanelet::Id of this element diff --git a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp index 59c6059ff8e..cf9ba2db422 100755 --- a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp +++ b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp @@ -60,7 +60,7 @@ LineStrings3d CarmaTrafficSignal::stopLine() return getParameters(RoleName::RefLine); } -Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) +Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) { auto sl = stopLine(); if (sl.empty()) { @@ -78,15 +78,15 @@ Optional CarmaTrafficSignal::getStopLine(const ConstLanelet& llt) return sl.at(size_t(std::distance(llts.begin(), it))); } -Optional CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt) +Optional CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt) { Optional mutable_stop_line = getStopLine(llt); - + if (!mutable_stop_line) return boost::none; ConstLineString3d const_stop_line = mutable_stop_line.get(); - + return const_stop_line; } @@ -119,14 +119,14 @@ boost::optional> Ca LOG_WARN_STREAM("CarmaTrafficSignal doesn't have any recorded states of traffic lights"); return boost::none; } - + if (lanelet::time::toSec(fixed_cycle_duration) < 1.0) // there are recorded states, but no fixed_cycle_duration means it is dynamic { if (recorded_time_stamps.size() != recorded_start_time_stamps.size()) { throw std::invalid_argument("recorded_start_time_stamps size is not equal to recorded_time_stamps size"); } - + // if requested time is BEFORE recorded states' time interval, return STOP_AND_REMAIN if (recorded_start_time_stamps.front() >= time_stamp) { @@ -136,11 +136,11 @@ boost::optional> Ca // if requested time is AFTER recorded states' time interval, return STOP_AND_REMAIN if (recorded_time_stamps.back().first <= time_stamp) { - auto end_infinity_time = timeFromSec(2147483647); //corresponds to 03:14:07 on Tuesday, 19 January 2038. + auto end_infinity_time = timeFromSec(INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES); //corresponds to 03:14:07 on Tuesday, 19 January 2038. LOG_DEBUG_STREAM("CarmaTrafficSignal doesn't have enough state saved, so returning STOP_AND_REMAIN state! End_time: " << end_infinity_time); return std::pair(end_infinity_time, CarmaTrafficSignalState::STOP_AND_REMAIN); } - + // iterate through states in the dynamic states to get the signal. for (size_t i = 0; i < recorded_time_stamps.size(); i++) { @@ -150,7 +150,7 @@ boost::optional> Ca } } } - + // This part of the code is used for predicting state if fixed_cycle_duration is set using setStates function // shift starting time to the future or to the past to fit input into a valid cycle boost::posix_time::time_duration accumulated_offset_duration; @@ -158,8 +158,8 @@ boost::optional> Ca int num_of_cycles = std::abs(lanelet::time::toSec(recorded_time_stamps.front().first - time_stamp) / lanelet::time::toSec(fixed_cycle_duration)); accumulated_offset_duration = durationFromSec( num_of_cycles * lanelet::time::toSec(fixed_cycle_duration)); - - if (offset_duration_dir < 0) + + if (offset_duration_dir < 0) { while (recorded_time_stamps.front().first - accumulated_offset_duration > time_stamp) { @@ -171,7 +171,7 @@ boost::optional> Ca { double end_time = lanelet::time::toSec(recorded_time_stamps[i].first) + offset_duration_dir * lanelet::time::toSec(accumulated_offset_duration); if (end_time >= lanelet::time::toSec(time_stamp)) - { + { return std::pair(timeFromSec(end_time), recorded_time_stamps[i].second); } } @@ -182,7 +182,7 @@ boost::optional> Ca lanelet::ConstLanelets CarmaTrafficSignal::getControlStartLanelets() const { return getParameters(CarmaTrafficSignalRoleNameString::ControlStart); -} +} lanelet::ConstLanelets CarmaTrafficSignal::getControlEndLanelets() const { From 9fd79b22981c25694aee7019a60c04c3050edbca Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 18 Dec 2023 12:24:19 -0500 Subject: [PATCH 2/4] rename --- .../regulatory_elements/CarmaTrafficSignal.h | 4 ++++ common/lanelet2_extension/lib/CarmaTrafficSignal.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index fd779b34dea..dfa61907c0c 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -53,8 +53,12 @@ enum class CarmaTrafficSignalState { CAUTION_CONFLICTING_TRAFFIC=9 }; +namespace time{ + const int INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES = 2147483647; +} + struct CarmaTrafficSignalRoleNameString { static constexpr char ControlStart[] = "control_start"; diff --git a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp index cf9ba2db422..7436f1955a7 100755 --- a/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp +++ b/common/lanelet2_extension/lib/CarmaTrafficSignal.cpp @@ -136,7 +136,7 @@ boost::optional> Ca // if requested time is AFTER recorded states' time interval, return STOP_AND_REMAIN if (recorded_time_stamps.back().first <= time_stamp) { - auto end_infinity_time = timeFromSec(INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES); //corresponds to 03:14:07 on Tuesday, 19 January 2038. + auto end_infinity_time = timeFromSec(time::INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES); //corresponds to 03:14:07 on Tuesday, 19 January 2038. LOG_DEBUG_STREAM("CarmaTrafficSignal doesn't have enough state saved, so returning STOP_AND_REMAIN state! End_time: " << end_infinity_time); return std::pair(end_infinity_time, CarmaTrafficSignalState::STOP_AND_REMAIN); } From 17fa7dd977383931b0f68ed8958122044fcfe037 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Wed, 20 Dec 2023 04:00:16 -0500 Subject: [PATCH 3/4] pr comments --- .../lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index dfa61907c0c..91c738e57aa 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -55,7 +55,7 @@ enum class CarmaTrafficSignalState { namespace time{ -const int INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES = 2147483647; +constexpr auto INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES{2147483647}; } From 06967c9424e8ef9680e9d3915d1c26cd7bd5bba9 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Wed, 20 Dec 2023 15:27:48 -0500 Subject: [PATCH 4/4] comments --- .../regulatory_elements/CarmaTrafficSignal.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h index 91c738e57aa..bad4c6abefc 100644 --- a/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h +++ b/common/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h @@ -55,6 +55,9 @@ enum class CarmaTrafficSignalState { namespace time{ +//This int is max representable number by 32bit which the posix::time library is using. +//It corresponds to 03:14:07 on Tuesday, 19 January 2038. + constexpr auto INFINITY_END_TIME_FOR_NOT_ENOUGH_STATES{2147483647}; }