- 3.0.2 core module.
Switching.h
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4 **********************************************************************************************************************/
5 
6 #pragma once
7 
8 namespace ct {
9 namespace core {
11 template <class T, class Alloc = Eigen::aligned_allocator<T>>
12 using Switched = std::vector<T, Alloc>;
13 
15 template <class Phase, typename Time>
17 {
18  Phase pre_phase;
19  Phase post_phase;
21 };
22 
24 
38 template <class Phase, typename Time>
40 {
41 public:
42  typedef std::vector<Phase> PhaseSchedule_t;
43  typedef std::vector<Time> TimeSchedule_t;
44 
46  PhaseSequence(const Time& start_time = 0) { time_schedule_.push_back(start_time); }
50  void addPhase(Phase phase, Time duration)
51  {
52  phase_schedule_.push_back(std::move(phase));
53  time_schedule_.emplace_back(time_schedule_.back() + duration);
54  }
56  std::size_t getNumPhases() const { return phase_schedule_.size(); }
58  std::size_t getNumSwitches() const { return getNumPhases() - 1; }
60  Time getTotalDuration() const { return time_schedule_.back() - time_schedule_.front(); }
62  Time getStartTimeFromIdx(std::size_t idx) const { return time_schedule_[idx]; }
64  Time getEndTimeFromIdx(std::size_t idx) const { return time_schedule_[idx + 1]; }
66  Phase getPhaseFromIdx(std::size_t idx) const { return phase_schedule_[idx]; }
68  Phase getPhaseFromTime(Time time) const { return getPhaseFromIdx(getIdxFromTime(time)); }
70  Phase getFirstPhase() const { return phase_schedule_.front(); };
72  Phase getFinalPhase() const { return phase_schedule_.back(); };
75  {
76  return {getPhaseFromIdx(idx), getPhaseFromIdx(idx + 1), getEndTimeFromIdx(idx)};
77  }
80  {
81  return getSwitchEventFromIdx(getIdxFromTime(time));
82  }
84  std::size_t getIdxFromTime(Time time) const
85  {
86  // Finds pointer to first element less or equal to time
87  // i.e. it returns the index for the phase with time in [t_start, t_end)
88  // times outside the vector are mapped to the first and last phase
89  if (time < time_schedule_.front())
90  {
91  return 0;
92  }
93  else if (time >= time_schedule_.back())
94  {
95  return getNumPhases() - 1;
96  }
97  else
98  {
99  auto up = std::upper_bound(time_schedule_.begin(), time_schedule_.end(), time);
100  return up - time_schedule_.begin() - 1;
101  }
102  }
103 
104 private:
105  PhaseSchedule_t phase_schedule_;
106  TimeSchedule_t time_schedule_;
107 };
108 
111 
114 
115 } // namespace core
116 } // namespace ct
std::vector< T, Alloc > Switched
Declaring Switched alias such that we can write Switched<System>
Definition: Switching.h:12
Time getEndTimeFromIdx(std::size_t idx) const
get end time from sequence index
Definition: Switching.h:64
~PhaseSequence()
Destructor.
Definition: Switching.h:48
Describes a switch between phases.
Definition: Switching.h:16
SwitchEvent< Phase, Time > getSwitchEventFromIdx(std::size_t idx) const
get next switch event from sequence index
Definition: Switching.h:74
Phase post_phase
Definition: Switching.h:19
std::vector< Phase > PhaseSchedule_t
Definition: Switching.h:42
void addPhase(Phase phase, Time duration)
add a phase with duration
Definition: Switching.h:50
Phase getPhaseFromIdx(std::size_t idx) const
get phase pointer from sequence index
Definition: Switching.h:66
Time switch_time
Definition: Switching.h:20
PhaseSequence(const Time &start_time=0)
Construct empty sequence (with default start time)
Definition: Switching.h:46
Phase getFinalPhase() const
get Final phase pointer
Definition: Switching.h:72
Time getTotalDuration() const
get sequence total duration
Definition: Switching.h:60
Describes a Phase sequence with timing.
Definition: Switching.h:39
SwitchEvent< Phase, Time > getSwitchEventFromTime(Time time) const
get next switch event from time
Definition: Switching.h:79
Time getStartTimeFromIdx(std::size_t idx) const
get start time from sequence index
Definition: Switching.h:62
Phase getPhaseFromTime(Time time) const
get phase pointer from time
Definition: Switching.h:68
std::vector< Time > TimeSchedule_t
Definition: Switching.h:43
Phase getFirstPhase() const
get First phase pointer
Definition: Switching.h:70
std::size_t getIdxFromTime(Time time) const
get sequence index from time
Definition: Switching.h:84
Phase pre_phase
Definition: Switching.h:18
std::size_t getNumPhases() const
get number of phases
Definition: Switching.h:56
std::size_t getNumSwitches() const
get number of switches
Definition: Switching.h:58
double Time
Definition: Time.h:11