Helios++
Helios software for LiDAR simulations
Platform.h
1 #pragma once
2 
3 
4 #include <fstream>
5 
6 #include <glm/glm.hpp>
7 
8 #include "Asset.h"
9 #include "PlatformSettings.h"
10 #include "maths/Rotation.h"
11 #include "Scene.h"
12 #include <NoiseSource.h>
13 #include "Directions.h"
14 #ifdef PYTHON_BINDING
15 #include <PythonDVec3.h>
16 #endif
17 
21 class Platform : public Asset {
22 public:
23  // *** ATTRIBUTES *** //
24  // ******************** //
25  // platform configuration
29  glm::dvec3 cfg_device_relativeMountPosition = glm::dvec3(0, 0, 0);
34 
35  // misc stuff
39  double lastCheckZ = 0;
43  glm::dvec3 lastGroundCheck = glm::dvec3(0, 0, 0);
47  std::shared_ptr<Scene> scene = nullptr;
48 
49  // Noise generators
53  std::shared_ptr<NoiseSource<double>> positionXNoiseSource = nullptr;
57  std::shared_ptr<NoiseSource<double>> positionYNoiseSource = nullptr;
61  std::shared_ptr<NoiseSource<double>> positionZNoiseSource = nullptr;
65  std::shared_ptr<NoiseSource<double>> attitudeXNoiseSource = nullptr;
69  std::shared_ptr<NoiseSource<double>> attitudeYNoiseSource = nullptr;
73  std::shared_ptr<NoiseSource<double>> attitudeZNoiseSource = nullptr;
74 
75  // Output file writer stuff
79  double dmax = std::numeric_limits<double>::max();
83  glm::dvec3 prevWrittenPos = glm::dvec3(dmax, dmax, dmax);
84 
85  // Platform Settings
94  glm::dvec3 originWaypoint = glm::dvec3(0, 0, 0);
98  glm::dvec3 targetWaypoint = glm::dvec3(0, 0, 0);
103  glm::dvec3 nextWaypoint = glm::dvec3(0, 0, 0);
104 
109  bool onGround = false;
115  bool stopAndTurn = false;
121  bool smoothTurn = false;
127  bool slowdownEnabled = true;
128 
129  // State variables
133  glm::dvec3 position = glm::dvec3(0, 0, 0);
138 
147  bool writeNextTrajectory = true;
148 
149 
150  // *** CACHE ATTRIBUTES *** //
151  // ************************** //
155  glm::dvec3 cached_absoluteMountPosition = glm::dvec3(0, 0, 0);
159  Rotation cached_absoluteMountAttitude = Rotation(glm::dvec3(0, 1, 0), 0);
160 
164  glm::dvec3 cached_dir_current = glm::dvec3(0, 0, 0);
168  glm::dvec3 cached_dir_current_xy = glm::dvec3(0, 0, 0);
169 
173  glm::dvec3 cached_vectorToTarget = glm::dvec3(0, 0, 0);
178  glm::dvec3 cached_vectorToTarget_xy = glm::dvec3(0, 0, 0);
179 
184 
189  glm::dvec3 cached_originToTargetDir_xy = glm::dvec3(0, 0, 0);
194  glm::dvec3 cached_targetToNextDir_xy = glm::dvec3(0, 0, 0);
215 
216 
217  // *** CONSTRUCTION / DESTRUCTION *** //
218  // ************************************ //
222  Platform() = default;
223  virtual std::shared_ptr<Platform> clone();
224  virtual void _clone(std::shared_ptr<Platform> p);
225 
226 
227  // *** M E T H O D S *** //
228  // *********************** //
233  virtual void prepareSimulation(int simFrequency_hz) {updateStaticCache();}
239  virtual void applySettings(
240  std::shared_ptr<PlatformSettings> settings,
241  bool manual
242  );
243 
249  virtual void updateStaticCache();
256  void updateDynamicCache();
263  virtual bool waypointReached();
264 
270  virtual void doSimStep(int simFrequency_hz){}
271 
275  virtual void initLegManual() {}
276 
280  virtual void initLeg() {}
281 
282 
283  // *** GETTERS and SETTERS *** //
284  // ***************************** //
289  virtual void setAttitude(Rotation attitude);
294  virtual void setOrigin(glm::dvec3 origin);
299  virtual void setDestination(glm::dvec3 dest);
304  virtual void setAfterDestination(glm::dvec3 next);
309  void setPosition(glm::dvec3 pos);
314  void setOffset(glm::dvec3 pos);
315 
322  }
323 
330  }
331 
337  return this->attitude;
338  }
349  return this->attitude;
350  }
354  virtual glm::dvec3 getCurrentDirection(){
356  }
357 
362  glm::dvec3 getPosition() {
363  return position;
364  }
365 
370  glm::dvec3 getVectorToTarget() {
371  return cached_vectorToTarget;
372  }
373 
381  virtual inline void getRollPitchYaw(
382  double &roll, double &pitch, double &yaw
383  ) {attitude.getAngles(&RotationOrder::XYZ, roll, pitch, yaw);}
384 
390  virtual void setHeadingRad(double rad) {}
396  virtual double getHeadingRad() {return 0;}
397 
402  virtual glm::dvec3 getVelocity() {return glm::dvec3(0,0,0);}
407  virtual bool canMove() {return false;}
413  virtual bool canStopAndTurn() {return false;}
414 
415 };
std::shared_ptr< NoiseSource< double > > positionZNoiseSource
Noise source for z component of platform position.
Definition: Platform.h:61
glm::dvec3 targetWaypoint
Target waypoint (destination)
Definition: Platform.h:98
bool smoothTurn
Flag to specify if platform must work in smooth turn mode (true) or not (false). Not all platforms su...
Definition: Platform.h:121
Rotation getAbsoluteMountAttitude()
Obtain platform absolute mount attitude.
Definition: Platform.h:320
glm::dvec3 cached_originToTargetDir_xy
Director vector from origin to target over the XY plane (z is always 0).
Definition: Platform.h:189
Class representing a platform asset.
Definition: Platform.h:21
glm::dvec3 applyTo(glm::dvec3 u)
Definition: Rotation.cpp:250
double cached_targetToNextAngle_xy
Angle in which identifies director vector from target waypoint to waypoint after target...
Definition: Platform.h:214
double cached_endTargetAngle_xy
Angle between director vector from origin to target and director vector from target to waypoint after...
Definition: Platform.h:199
virtual void updateStaticCache()
Cache update which only needs to be performed after static modifications. Updating static cache also ...
Definition: Platform.cpp:100
virtual void initLeg()
Function to initialize leg when working in default mode.
Definition: Platform.h:280
void updateDynamicCache()
Cache update which must be performed between simulation steps and after static modifications. Updating static cache also updates dynamic cache but updating dynamic cache does not update static cache.
Definition: Platform.cpp:131
void setPosition(glm::dvec3 pos)
Set platform position.
Definition: Platform.cpp:95
glm::dvec3 cached_vectorToTarget_xy
Distance vector from current position to target over XY plane (z is always 0)
Definition: Platform.h:178
Base class for all assets.
Definition: Asset.h:10
std::shared_ptr< Scene > scene
Scene where the platform belongs to.
Definition: Platform.h:47
Definition: Rotation.h:80
virtual void setAttitude(Rotation attitude)
Set platform attitude.
Definition: Platform.cpp:76
virtual void doSimStep(int simFrequency_hz)
Do corresponding computations for the platform at current simulation step.
Definition: Platform.h:270
glm::dvec3 position
Platform 3D position.
Definition: Platform.h:133
virtual bool waypointReached()
Check if platform has reached its destination way point (true) or not (false)
Definition: Platform.cpp:151
std::shared_ptr< NoiseSource< double > > attitudeXNoiseSource
Noise source for x component of platform attitude.
Definition: Platform.h:65
glm::dvec3 cached_dir_current
Current director vector over XY plane.
Definition: Platform.h:164
glm::dvec3 prevWrittenPos
Not used at the moment. Might be removed in the future.
Definition: Platform.h:83
double lastCheckZ
Not used at the moment. Might be removed in the future.
Definition: Platform.h:39
std::shared_ptr< NoiseSource< double > > positionXNoiseSource
Noise source for x component of platform position.
Definition: Platform.h:53
double dmax
Not used at the moment. Might be removed in the future.
Definition: Platform.h:79
static const glm::dvec3 forward
y : forward-backward direction
Definition: Directions.h:21
virtual void setHeadingRad(double rad)
Set the heading angle in radians. This angle can be understood as yaw in most cases.
Definition: Platform.h:390
bool writeNextTrajectory
Flag to specify if next trajectory needs to be written (true) or not (false)
Definition: Platform.h:147
bool stopAndTurn
Flag to specify if platform must work in stop and turn mode (true) or not (false). Not all platforms support this mode, so it will only be used when possible.
Definition: Platform.h:115
virtual bool canStopAndTurn()
Check if platform support stop and turn mode (true) or not (false)
Definition: Platform.h:413
Rotation cached_absoluteMountAttitude
Cached absolute mount attitude.
Definition: Platform.h:159
virtual void initLegManual()
Function to initialize leg when working in manual mode.
Definition: Platform.h:275
std::shared_ptr< NoiseSource< double > > positionYNoiseSource
Noise source for y component of platform position.
Definition: Platform.h:57
glm::dvec3 cached_targetToNextDir_xy
Director vector from target to after target waypoint over the XY plane (z is always 0)...
Definition: Platform.h:194
Platform()=default
Default platform constructor.
virtual glm::dvec3 getVelocity()
Obtain platform velocity vector.
Definition: Platform.h:402
bool mSetOrientationOnLegInit
Not used at the moment. Might be removed in the future.
Definition: Platform.h:142
virtual void setOrigin(glm::dvec3 origin)
Set platform origin way point.
Definition: Platform.cpp:83
double cached_distanceToTarget_xy
Distance on XY plane between current position and target.
Definition: Platform.h:183
void setOffset(glm::dvec3 pos)
Set platform position.
glm::dvec3 cfg_device_relativeMountPosition
Device mount position relative to the platform.
Definition: Platform.h:29
virtual void applySettings(std::shared_ptr< PlatformSettings > settings, bool manual)
Apply given platform settings to the platform.
Definition: Platform.cpp:68
virtual bool canMove()
Check if platform can move (true) or not (false)
Definition: Platform.h:407
glm::dvec3 cached_vectorToTarget
Distance vector from current position to target.
Definition: Platform.h:173
double cached_currentAngle_xy
Angle between current director vector and director vector from target to waypoint after target...
Definition: Platform.h:204
Rotation cfg_device_relativeMountAttitude
Device mount attitude relative to the platform.
Definition: Platform.h:33
glm::dvec3 getPosition()
Obtain platform position.
Definition: Platform.h:362
glm::dvec3 lastGroundCheck
Not used at the moment. Might be removed in the future.
Definition: Platform.h:43
static const RotationOrder XYZ
Definition: RotationOrder.h:48
bool onGround
Flag to specify if the platform must be placed on ground (true) or not (false)
Definition: Platform.h:109
virtual glm::dvec3 getCurrentDirection()
Obtain platform current direction.
Definition: Platform.h:354
virtual void setAfterDestination(glm::dvec3 next)
Set platform after destination way point.
Definition: Platform.cpp:91
Rotation attitude
Platform 3D attitude.
Definition: Platform.h:137
bool slowdownEnabled
Flag to specify if slowdown stage must be enabled (true) or not (false). Not all platforms have a slo...
Definition: Platform.h:127
double cached_originToTargetAngle_xy
Angle in which identifies director vector from origin waypoint to target waypoint.
Definition: Platform.h:209
glm::dvec3 nextWaypoint
Waypoint after target. For the last target, waypoint after target is equal to the target itself...
Definition: Platform.h:103
glm::dvec3 cached_dir_current_xy
Current director vector over XY plane (z is always 0)
Definition: Platform.h:168
glm::dvec3 originWaypoint
Origin waypoint.
Definition: Platform.h:94
virtual void prepareSimulation(int simFrequency_hz)
Prepare the platform to deal with simulation.
Definition: Platform.h:233
double cfg_settings_movePerSec_m
How meters per seconds the platform moves. NOTICE this behavior must be overridden by platforms imple...
Definition: Platform.h:90
virtual void getRollPitchYaw(double &roll, double &pitch, double &yaw)
Obtain platform roll, pitch and yaw angles. Notice not all platforms track those angles.
Definition: Platform.h:381
std::shared_ptr< NoiseSource< double > > attitudeYNoiseSource
Noise source for y component of platform attitude.
Definition: Platform.h:69
void getAngles(RotationOrder const *order, double &roll, double &pitch, double &yaw)
Get the roll, pitch and yaw for the Rotation.
Definition: Rotation.cpp:360
Rotation getAttitude()
Obtain platform attitude.
Definition: Platform.h:336
glm::dvec3 cached_absoluteMountPosition
Cached absolute mount position.
Definition: Platform.h:155
virtual void setDestination(glm::dvec3 dest)
Set platform destination way point.
Definition: Platform.cpp:87
glm::dvec3 getAbsoluteMountPosition()
Obtain platform absolute mount position.
Definition: Platform.h:328
virtual double getHeadingRad()
Obtain platform heading angle in radians, which can be understood as yaw in most cases.
Definition: Platform.h:396
static const glm::dvec3 up
z : up-down direction
Definition: Directions.h:25
std::shared_ptr< NoiseSource< double > > attitudeZNoiseSource
Noise source for z component of platform attitude.
Definition: Platform.h:73
virtual Rotation getDirectionalAttitude()
Obtain the directional attitude. While the attitude represents the platform orientation, the directional attitude represents the movement direction.
Definition: Platform.h:348
glm::dvec3 getVectorToTarget()
Obtain platform vector to target (cache)
Definition: Platform.h:370