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..bad4c6abefc 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,15 @@ enum class CarmaTrafficSignalState { CAUTION_CONFLICTING_TRAFFIC=9 }; +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}; + +} + struct CarmaTrafficSignalRoleNameString { static constexpr char ControlStart[] = "control_start"; @@ -67,7 +76,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 +88,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 +107,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 +124,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 +133,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..7436f1955a7 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(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); } - + // 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 {