#include <GeofenceSchedule.hpp>

Collaboration diagram for carma_wm_ctrl::GeofenceSchedule:

Public Types
using	DayOfTheWeekSet = std::unordered_set< boost::gregorian::greg_weekday, std::hash< int > >

Public Member Functions
	GeofenceSchedule ()
	Default Constructor does not initialize any members. More...

	GeofenceSchedule (rclcpp::Time schedule_start, rclcpp::Time schedule_end, rclcpp::Duration control_start, rclcpp::Duration control_duration, rclcpp::Duration control_offset, rclcpp::Duration control_span, rclcpp::Duration control_period, DayOfTheWeekSet week_day_set={ 0, 1, 2, 3, 4, 5, 6 })
	Constructor. More...

bool	scheduleExpired (const rclcpp::Time &time) const
	Returns true if the schedule has expired by the provided time. More...

bool	scheduleStarted (const rclcpp::Time &time) const
	Returns true if the schedule has started by the provided time. More...

std::pair< bool, rclcpp::Time >	getNextInterval (const rclcpp::Time &time) const
	Returns the start time of the next active interval defined by this schedule. More...

Public Attributes
rclcpp::Time	schedule_start_

rclcpp::Time	schedule_end_

rclcpp::Duration	control_start_ {0, 0}

rclcpp::Duration	control_duration_ {0, 0}

rclcpp::Duration	control_offset_ {0, 0}

rclcpp::Duration	control_span_ {0, 0}

rclcpp::Duration	control_period_ {0, 0}

DayOfTheWeekSet	week_day_set_

Detailed Description

Definition at line 26 of file GeofenceSchedule.hpp.

Member Typedef Documentation

◆ DayOfTheWeekSet

using carma_wm_ctrl::GeofenceSchedule::DayOfTheWeekSet = std::unordered_set<boost::gregorian::greg_weekday, std::hash<int> >

Definition at line 38 of file GeofenceSchedule.hpp.

Constructor & Destructor Documentation

◆ GeofenceSchedule() [1/2]

carma_wm_ctrl::GeofenceSchedule::GeofenceSchedule ( )

Default Constructor does not initialize any members.

Definition at line 22 of file GeofenceSchedule.cpp.

23{

24}

◆ GeofenceSchedule() [2/2]

carma_wm_ctrl::GeofenceSchedule::GeofenceSchedule	(	rclcpp::Time	schedule_start,
		rclcpp::Time	schedule_end,
		rclcpp::Duration	control_start,
		rclcpp::Duration	control_duration,
		rclcpp::Duration	control_offset,
		rclcpp::Duration	control_span,
		rclcpp::Duration	control_period,
		DayOfTheWeekSet	week_day_set = `{ 0, 1, 2, 3, 4, 5, 6 }`
	)

Constructor.

Parameters

schedule_start	Overall schedule start in UTC
schedule_end	Overall schedule end in UTC
control_start	Duration from start of day
control_duration	Duration from start of control_start
control_offset	Duration from start of day (specifies start time for repeat parameters - span, period)
control_span	Duration of active status. Starts from offset
control_period	Interval between active status within control_duration since control_start
week_day_set	Set of days of the week which this schedule applies to. Defaults as all days of the week

Definition at line 26 of file GeofenceSchedule.cpp.

                                                                                                                            :
                                   schedule_start_(schedule_start), schedule_end_(schedule_end),
                                   control_start_ (control_start), control_duration_ (control_duration),
                                   control_offset_ (control_offset), control_span_ (control_span), control_period_ (control_period), week_day_set_(week_day_set)
{}

Member Function Documentation

◆ getNextInterval()

std::pair< bool, rclcpp::Time > carma_wm_ctrl::GeofenceSchedule::getNextInterval ( const rclcpp::Time & time ) const

Returns the start time of the next active interval defined by this schedule.

Parameters

time	UTC time to compare

Returns: Returns a pair with the following semantics First Element: A boolean indicating if the provided time is within a currently active control period Second Element: The start time of the next scheduled active control interval within the current day. Returns rclcpp::Time(0) when the schedule is expired or the next interval will be on a different day of the week or after schedule end

TODO the UTC offset is provided in the geofence spec but for now we will ignore and assume all times are UTC

Definition at line 46 of file GeofenceSchedule.cpp.

{
  auto clock_type = time.get_clock_type();
  if (scheduleExpired(time))
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule expired");
    return std::make_pair(false, rclcpp::Time(0, 0, clock_type));  // If the schedule has expired or was never started
  }
  
  boost::posix_time::ptime boost_time = boost::posix_time::from_time_t(time.seconds());
  boost_time += boost::posix_time::microseconds((int)((time.seconds()-std::floor(time.seconds()))*1e6));
  boost::gregorian::date date = boost_time.date();
 
  if (week_day_set_.find(date.day_of_week()) == week_day_set_.end())
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence wrong day of the week");
    return std::make_pair(false, rclcpp::Time(0, 0, clock_type));  // This geofence is not active on this day
  }
 
  auto time_of_day = boost_time.time_of_day();
 
 
  // Convert schedule into workable components
  boost::posix_time::ptime ptime_start_of_day(date, boost::posix_time::hours(0));  // Get absolute start time of the day
  
  rclcpp::Time ros_time_of_day = rclcpp::Time(lanelet::time::toSec(time_of_day) * 1e9, clock_type);
 
  rclcpp::Time abs_day_start = rclcpp::Time(lanelet::time::toSec(ptime_start_of_day) * 1e9, clock_type);
  rclcpp::Duration cur_start = control_start_ + control_offset_; // accounting for the shift of repetition start
 
  // Check if current time is after end of control
  if (ros_time_of_day > rclcpp::Time((control_start_ + control_duration_).nanoseconds(), clock_type))
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule too late in the day");
    // The requested time is after control end so there will not be another interval
    return std::make_pair(false, rclcpp::Time(0, 0, clock_type));
  }
 
  // Iterate over the day to find the next control interval
  constexpr int num_sec_in_day = 86400;
  const rclcpp::Duration full_day(num_sec_in_day* 1e9);
  bool time_in_active_period = false;  // Flag indicating if the requested time is within an active control span
 
  while (cur_start < full_day && ros_time_of_day > rclcpp::Time(cur_start.nanoseconds(), clock_type))
  {
    // Check if the requested time is within the control period being evaluated
    if (rclcpp::Time(cur_start.nanoseconds(), clock_type) < ros_time_of_day &&
        ros_time_of_day < rclcpp::Time((cur_start + control_span_).nanoseconds(), clock_type))
    {
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule active!");
      
      time_in_active_period = true;
    }
    cur_start = cur_start + control_period_;
 
  }
 
 
  // check if the only next interval is after the schedule end or past the end of the day
  if (abs_day_start + cur_start > schedule_end_ || cur_start > full_day || cur_start > (control_start_ + control_duration_))
  {
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule beyond end time");
    return std::make_pair(time_in_active_period, rclcpp::Time(0, 0, clock_type));
  }
 
  // At this point we should have the next start time which is still within the schedule and day
  return std::make_pair(time_in_active_period, abs_day_start + cur_start);
}