Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/green signal issue #261

Merged
merged 4 commits into from
Dec 20, 2023
Merged

Fix/green signal issue #261

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c20f131
add end time
MishkaMN Dec 18, 2023
9fd79b2
rename
MishkaMN Dec 18, 2023
17fa7dd
pr comments
MishkaMN Dec 20, 2023
06967c9
comments
MishkaMN Dec 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 20 additions & 11 deletions ...on/lanelet2_extension/include/lanelet2_extension/regulatory_elements/CarmaTrafficSignal.h
View file
Open in desktop
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
Open in desktop
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
Loading