Helios++
Helios software for LiDAR simulations
InterpolatedMovingPlatform.h
1 #pragma once
2 
3 #include <platform/MovingPlatform.h>
4 #include <platform/trajectory/DesignTrajectoryFunction.h>
5 #include <fluxionum/TemporalDesignMatrix.h>
6 #include <fluxionum/DiffDesignMatrix.h>
7 #include <SimulationStepLoop.h>
8 #include <util/HeliosException.h>
9 #include <maths/MathConstants.h>
10 
11 #include <memory>
12 #include <functional>
13 
24 public:
29  enum class InterpolationScope{
30  POSITION,
31  ATTITUDE,
32  POSITION_AND_ATTITUDE
33  };
38  enum class RotationSpec{
39  CANONICAL,
40  ARINC_705
41  };
42 
43 protected:
44  // *** ATTRIBUTES *** //
45  // ******************** //
67  std::shared_ptr<DesignTrajectoryFunction> tf = nullptr;
74  arma::Col<double> timeFrontiers;
82  arma::Mat<double> frontierValues;
92  arma::Mat<double> frontierDerivatives;
114  std::function<Rotation(arma::Col<double> const)> calcAttitude;
121  std::function<
122  void(double &, double &, double &, Rotation &)
129  std::function<void(double const t)> doStepUpdates;
145  double startTime;
168 
169 public:
170  // *** CONSTRUCTION / DESTRUCTION *** //
171  // ************************************ //
192  TemporalDesignMatrix<double, double> const &tdm,
193  DiffDesignMatrix<double, double> const &ddm,
195  bool const syncGPSTime,
196  double const startTime,
197  RotationSpec rotspec=RotationSpec::ARINC_705
198  );
199  virtual ~InterpolatedMovingPlatform() = default;
200 
201  // *** M E T H O D S *** //
202  // *********************** //
206  void doSimStep(int simFrequency_hz) override;
224  bool waypointReached() override;
232  virtual void toTrajectoryTime(double const t);
233 
238  void initLegManual() override
240 
245  void initLeg() override
247 
248  // *** GETTERs and SETTERs *** //
249  // ***************************** //
255  {return stepLoop;}
261  {return scope;}
266  inline void setScope(InterpolationScope const scope)
267  {this->scope = scope;}
272  inline std::shared_ptr<DesignTrajectoryFunction>
274  {return tf;}
280  std::shared_ptr<DesignTrajectoryFunction> tf
281  ) {this->tf = tf;}
286  inline arma::Col<double> const & getTimeFrontiers() const
287  {return timeFrontiers;}
292  inline void setTimeFrontiers(arma::Col<double> const &timeFrontiers)
293  {this->timeFrontiers = timeFrontiers;}
298  inline arma::Mat<double> const & getFrontierValues() const
299  {return frontierValues;}
304  inline void setFrontierValues(arma::Mat<double> const &frontierValues)
305  {this->frontierValues = frontierValues;}
310  inline arma::Mat<double> const & getFrontierDerivatives() const
311  {return frontierDerivatives;}
317  arma::Mat<double> const &frontierDerivatives
318  ) {this->frontierDerivatives = frontierDerivatives;}
324  inline bool isSyncGPSTime() const {return syncGPSTime;}
330  inline void setSyncGPSTime(bool const syncGPSTime)
331  {this->syncGPSTime = syncGPSTime;}
336  inline double getStartTime() const {return startTime;}
341  inline void setStartTime(double const startTime)
342  {this->startTime = startTime;}
346  inline bool isInterpolated() const override {return true;}
351  inline double getCurrentLegStartTime() const {return currentLegStartTime;}
357  inline void setCurrentLegTimeDiff(double const timeDiff)
358  {currentLegTimeDiff = timeDiff;}
364  inline double getCurrentLegTimeDiff() const {return currentLegTimeDiff;}
365 
373  inline void getRollPitchYaw(
374  double &roll, double &pitch, double &yaw
375  ) override {
376  _getRollPitchYaw(roll, pitch, yaw, attitude);
377  }
378 };
Class representing a MovingPlatform which position is defined by a function interpolated from a Desig...
Definition: InterpolatedMovingPlatform.h:23
virtual void toTrajectoryTime(double const t)
Configures the iterative method of the trajectory function so the current iteration is considered to ...
Definition: InterpolatedMovingPlatform.cpp:114
InterpolationScope scope
Specify the scope of the interpolation.
Definition: InterpolatedMovingPlatform.h:57
InterpolationScope
Specify what is the interpolation scope. It is, which components are interpolated.
Definition: InterpolatedMovingPlatform.h:29
std::shared_ptr< DesignTrajectoryFunction > tf
The trajectory function defining the platform's motion.
Definition: InterpolatedMovingPlatform.h:67
void setTrajectoryFunction(std::shared_ptr< DesignTrajectoryFunction > tf)
Set the TrajectoryFunction.
Definition: InterpolatedMovingPlatform.h:279
void getRollPitchYaw(double &roll, double &pitch, double &yaw) override
Override the platform's default Platform::getRollPitchYaw method to account for the given rotation sp...
Definition: InterpolatedMovingPlatform.h:373
double getCurrentLegTimeDiff() const
Obtain the time difference between the start and end points of the current leg.
Definition: InterpolatedMovingPlatform.h:364
void initLeg() override
Consider the current time as the start time of the initialized leg.
Definition: InterpolatedMovingPlatform.h:245
void setStartTime(double const startTime)
Set the start time.
Definition: InterpolatedMovingPlatform.h:341
arma::Mat< double > frontierDerivatives
The vectors in such that at the -th frontier it is known that .
Definition: InterpolatedMovingPlatform.h:92
bool isInterpolated() const override
Definition: InterpolatedMovingPlatform.h:346
void setSyncGPSTime(bool const syncGPSTime)
Either enable (true) or disable (false) the synchronization of GPS time with platform's start time.
Definition: InterpolatedMovingPlatform.h:330
double getCurrentLegStartTime() const
Obtain the start time of the current leg.
Definition: InterpolatedMovingPlatform.h:351
std::function< void(double const t)> doStepUpdates
Function which handles the update of components belonging to interpolation scope at each simulation s...
Definition: InterpolatedMovingPlatform.h:129
void setFrontierValues(arma::Mat< double > const &frontierValues)
Set the values at the frontier points.
Definition: InterpolatedMovingPlatform.h:304
RotationSpec
Rotation specifications supported by the interpolated moving platform.
Definition: InterpolatedMovingPlatform.h:38
std::function< Rotation(arma::Col< double > const)> calcAttitude
Function which handles how attitude is calculated. It depends on the rotation specification.
Definition: InterpolatedMovingPlatform.h:114
std::shared_ptr< DesignTrajectoryFunction > getTrajectoryFunction() const
Obtain the TrajectoryFunction.
Definition: InterpolatedMovingPlatform.h:273
InterpolatedMovingPlatform(SimulationStepLoop &stepLoop, TemporalDesignMatrix< double, double > const &tdm, DiffDesignMatrix< double, double > const &ddm, InterpolationScope scope, bool const syncGPSTime, double const startTime, RotationSpec rotspec=RotationSpec::ARINC_705)
Build an InterpolatedMovingPlatform from given values and differentials. Note that the DiffDesignMatr...
Definition: InterpolatedMovingPlatform.cpp:9
double getStartTime() const
Obtain the start time.
Definition: InterpolatedMovingPlatform.h:336
std::function< void(double &, double &, double &, Rotation &) > _getRollPitchYaw
Function to get the roll, pitch, and yaw angles depending on the rotation specification.
Definition: InterpolatedMovingPlatform.h:123
arma::Mat< double > const & getFrontierDerivatives() const
Obtain the derivatives at the frontier points.
Definition: InterpolatedMovingPlatform.h:310
arma::Col< double > timeFrontiers
The time frontiers such that .
Definition: InterpolatedMovingPlatform.h:74
void initLegManual() override
Consider the current time as the start time of the manually initialized leg.
Definition: InterpolatedMovingPlatform.h:238
arma::Mat< double > frontierValues
The vectors in such that at the -th frontier it is known that .
Definition: InterpolatedMovingPlatform.h:82
RotationSpec rotspec
The rotation specification defining the interpolated attitude.
Definition: InterpolatedMovingPlatform.h:62
void setTimeFrontiers(arma::Col< double > const &timeFrontiers)
Set the time frontiers.
Definition: InterpolatedMovingPlatform.h:292
bool waypointReached() override
Check whether the interpolated moving platform has reached its waypoint or not.
Definition: InterpolatedMovingPlatform.cpp:109
void setCurrentLegTimeDiff(double const timeDiff)
Set the time difference between the start and end points of the current leg.
Definition: InterpolatedMovingPlatform.h:357
double startTime
The start time for the GPS time (in seconds).
Definition: InterpolatedMovingPlatform.h:145
SimulationStepLoop & getStepLoop() const
Obtain the reference to the SimulationStepLoop.
Definition: InterpolatedMovingPlatform.h:254
double currentLegStartTime
The time at the start of the current leg (in seconds). It is for the first leg and it is the time at...
Definition: InterpolatedMovingPlatform.h:154
arma::Col< double > const & getTimeFrontiers() const
Obtain the time frontiers.
Definition: InterpolatedMovingPlatform.h:286
SimulationStepLoop & stepLoop
Reference to the SimulationStepLoop defining the Simulation.
Definition: InterpolatedMovingPlatform.h:52
void setScope(InterpolationScope const scope)
Set the InterpolationScope.
Definition: InterpolatedMovingPlatform.h:266
InterpolationScope getScope() const
Obtain the InterpolationScope.
Definition: InterpolatedMovingPlatform.h:260
arma::Mat< double > const & getFrontierValues() const
Obtain the values at the frontier points.
Definition: InterpolatedMovingPlatform.h:298
bool syncGPSTime
If true, the GPS time will be synchronized with the start time of the InterpolatedMovingPlatform....
Definition: InterpolatedMovingPlatform.h:135
void doSimStep(int simFrequency_hz) override
Definition: InterpolatedMovingPlatform.cpp:106
void setFrontierDerivatives(arma::Mat< double > const &frontierDerivatives)
Set the derivatives at the frontier points.
Definition: InterpolatedMovingPlatform.h:316
double currentLegTimeDiff
The difference between the start time and the end time of the current leg.
Definition: InterpolatedMovingPlatform.h:167
bool isSyncGPSTime() const
Check whether the GPS time is requested to be synchronized with platform's start time (true) or not (...
Definition: InterpolatedMovingPlatform.h:324
Class representing a moving platform.
Definition: MovingPlatform.h:8
Rotation attitude
Platform 3D attitude.
Definition: Platform.h:139
Definition: Rotation.h:80
Class extending LinearVoidStepLoop to support main simulation loop.
Definition: SimulationStepLoop.h:15
double getCurrentTime() const
Obtain the virtual time (simulation time) that elapsed to reach current step.
Definition: SimulationStepLoop.h:67