Skip to content

Commit

Permalink
Fix/green signal issue (#261)
Browse files Browse the repository at this point in the history 
* add end time

* rename

* pr comments

* comments
  • Loading branch information
@MishkaMN
MishkaMN authored Dec 20, 2023
1 parent 15c867b commit 04c50b7
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 24 deletions.
31 changes: 20 additions & 11 deletions ...on/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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";
Expand All @@ -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
*/
Expand All @@ -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<boost::posix_time::ptime> recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps ,
std::vector<boost::posix_time::ptime> recorded_start_time_stamps; //user must ensure it's 1 to 1 with recorded_time_stamps ,
//used in dynamic SPAT processing
std::vector<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> recorded_time_stamps;
std::unordered_map<CarmaTrafficSignalState, boost::posix_time::time_duration> signal_durations;
Expand All @@ -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;

Expand All @@ -115,7 +124,7 @@ class CarmaTrafficSignal : public lanelet::RegulatoryElement
* @throw InvalidInputError if timestamps recorded somehow did not have full cycle
*/
boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> predictState(boost::posix_time::ptime time_stamp);

/**
* @brief Return the stop_lines related to the entry lanelets in order if exists.
*/
Expand All @@ -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<ConstLineString3d> getConstStopLine(const ConstLanelet& llt);
Optional<LineString3d> 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
Expand Down
26 changes: 13 additions & 13 deletions common/lanelet2_extension/lib/CarmaTrafficSignal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ LineStrings3d CarmaTrafficSignal::stopLine()
return getParameters<LineString3d>(RoleName::RefLine);
}

Optional<LineString3d> CarmaTrafficSignal::getStopLine(const ConstLanelet& llt)
Optional<LineString3d> CarmaTrafficSignal::getStopLine(const ConstLanelet& llt)
{
auto sl = stopLine();
if (sl.empty()) {
Expand All @@ -78,15 +78,15 @@ Optional<LineString3d> CarmaTrafficSignal::getStopLine(const ConstLanelet& llt)
return sl.at(size_t(std::distance(llts.begin(), it)));
}

Optional<ConstLineString3d> CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt)
Optional<ConstLineString3d> CarmaTrafficSignal::getConstStopLine(const ConstLanelet& llt)
{
Optional<LineString3d> mutable_stop_line = getStopLine(llt);

if (!mutable_stop_line)
return boost::none;

ConstLineString3d const_stop_line = mutable_stop_line.get();

return const_stop_line;
}

Expand Down Expand Up @@ -119,14 +119,14 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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)
{
Expand All @@ -136,11 +136,11 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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<boost::posix_time::ptime, CarmaTrafficSignalState>(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++)
{
Expand All @@ -150,16 +150,16 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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;
double offset_duration_dir = recorded_time_stamps.front().first > time_stamp ? -1.0 : 1.0; // -1 if past, +1 if time_stamp is in future

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)
{
Expand All @@ -171,7 +171,7 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> 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<boost::posix_time::ptime, CarmaTrafficSignalState>(timeFromSec(end_time), recorded_time_stamps[i].second);
}
}
Expand All @@ -182,7 +182,7 @@ boost::optional<std::pair<boost::posix_time::ptime, CarmaTrafficSignalState>> Ca
lanelet::ConstLanelets CarmaTrafficSignal::getControlStartLanelets() const
{
return getParameters<ConstLanelet>(CarmaTrafficSignalRoleNameString::ControlStart);
}
}

lanelet::ConstLanelets CarmaTrafficSignal::getControlEndLanelets() const
{
Expand Down

0 comments on commit 04c50b7

Please sign in to comment.