YARP
Yet Another Robot Platform

fakeIMU : fake device implementing the device interface typically implemented by an Inertial Measurement Unit More...

#include <fakeIMU/fakeIMU.h>

+ Inheritance diagram for fakeIMU:

Public Member Functions

 fakeIMU ()
 This device implements a fake analog sensor emulating an IMU. More...
 
 fakeIMU (const fakeIMU &)=delete
 
 fakeIMU (fakeIMU &&)=delete
 
fakeIMUoperator= (const fakeIMU &)=delete
 
fakeIMUoperator= (fakeIMU &&)=delete
 
 ~fakeIMU () override
 
bool open (yarp::os::Searchable &config) override
 Open the DeviceDriver. More...
 
bool close () override
 Close the DeviceDriver. More...
 
bool read (yarp::sig::Vector &out) override
 Read a vector from the sensor. More...
 
bool getChannels (int *nc) override
 Get the number of channels of the sensor. More...
 
bool calibrate (int ch, double v) override
 Calibrate the sensor, single channel. More...
 
size_t getNrOfThreeAxisGyroscopes () const override
 Get the number of three axis gyroscopes exposed by this sensor. More...
 
yarp::dev::MAS_status getThreeAxisGyroscopeStatus (size_t sens_index) const override
 Get the status of the specified sensor. More...
 
bool getThreeAxisGyroscopeName (size_t sens_index, std::string &name) const override
 Get the name of the specified sensor. More...
 
bool getThreeAxisGyroscopeFrameName (size_t sens_index, std::string &frameName) const override
 Get the name of the frame of the specified sensor. More...
 
bool getThreeAxisGyroscopeMeasure (size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
 Get the last reading of the gyroscope. More...
 
size_t getNrOfThreeAxisLinearAccelerometers () const override
 Get the number of three axis linear accelerometers exposed by this device. More...
 
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus (size_t sens_index) const override
 Get the status of the specified sensor. More...
 
bool getThreeAxisLinearAccelerometerName (size_t sens_index, std::string &name) const override
 Get the name of the specified sensor. More...
 
bool getThreeAxisLinearAccelerometerFrameName (size_t sens_index, std::string &frameName) const override
 Get the name of the frame of the specified sensor. More...
 
bool getThreeAxisLinearAccelerometerMeasure (size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
 Get the last reading of the specified sensor. More...
 
size_t getNrOfThreeAxisMagnetometers () const override
 Get the number of magnetometers exposed by this device. More...
 
yarp::dev::MAS_status getThreeAxisMagnetometerStatus (size_t sens_index) const override
 Get the status of the specified sensor. More...
 
bool getThreeAxisMagnetometerName (size_t sens_index, std::string &name) const override
 Get the name of the specified sensor. More...
 
bool getThreeAxisMagnetometerFrameName (size_t sens_index, std::string &frameName) const override
 Get the name of the frame of the specified sensor. More...
 
bool getThreeAxisMagnetometerMeasure (size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
 Get the last reading of the specified sensor. More...
 
size_t getNrOfOrientationSensors () const override
 Get the number of orientation sensors exposed by this device. More...
 
yarp::dev::MAS_status getOrientationSensorStatus (size_t sens_index) const override
 Get the status of the specified sensor. More...
 
bool getOrientationSensorName (size_t sens_index, std::string &name) const override
 Get the name of the specified sensor. More...
 
bool getOrientationSensorFrameName (size_t sens_index, std::string &frameName) const override
 Get the name of the frame of the specified sensor. More...
 
bool getOrientationSensorMeasureAsRollPitchYaw (size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
 Get the last reading of the orientation sensor as roll pitch yaw. More...
 
- Public Member Functions inherited from yarp::dev::DeviceDriver
 DeviceDriver ()
 
 DeviceDriver (const DeviceDriver &other)=delete
 
 DeviceDriver (DeviceDriver &&other) noexcept=delete
 
DeviceDriveroperator= (const DeviceDriver &other)=delete
 
DeviceDriveroperator= (DeviceDriver &&other) noexcept=delete
 
 ~DeviceDriver () override
 
virtual std::string id () const
 Return the id assigned to the PolyDriver. More...
 
virtual void setId (const std::string &id)
 Set the id for this device. More...
 
template<class T >
bool view (T *&x)
 Get an interface to the device driver. More...
 
virtual DeviceDrivergetImplementation ()
 Some drivers are bureaucrats, pointing at others. More...
 
- Public Member Functions inherited from yarp::os::IConfig
virtual ~IConfig ()
 Destructor. More...
 
virtual bool configure (Searchable &config)
 Change online parameters. More...
 
- Public Member Functions inherited from yarp::dev::IGenericSensor
virtual ~IGenericSensor ()
 
- Public Member Functions inherited from yarp::os::PeriodicThread
 PeriodicThread (double period, ShouldUseSystemClock useSystemClock=ShouldUseSystemClock::No, PeriodicThreadClock clockAccuracy=PeriodicThreadClock::Relative)
 Constructor. More...
 
 PeriodicThread (double period, PeriodicThreadClock clockAccuracy)
 Constructor. More...
 
virtual ~PeriodicThread ()
 
bool start ()
 Call this to start the thread. More...
 
void step ()
 Call this to "step" the thread rather than starting it. More...
 
void stop ()
 Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() called). More...
 
void askToStop ()
 Stop the thread. More...
 
bool isRunning () const
 Returns true when the thread is started, false otherwise. More...
 
bool isSuspended () const
 Returns true when the thread is suspended, false otherwise. More...
 
bool setPeriod (double period)
 Set the (new) period of the thread. More...
 
double getPeriod () const
 Return the current period of the thread. More...
 
void suspend ()
 Suspend the thread, the thread keeps running by doLoop is never executed. More...
 
void resume ()
 Resume the thread if previously suspended. More...
 
void resetStat ()
 Reset thread statistics. More...
 
double getEstimatedPeriod () const
 Return estimated period since last reset. More...
 
void getEstimatedPeriod (double &av, double &std) const
 Return estimated period since last reset. More...
 
unsigned int getIterations () const
 Return the number of iterations performed since last reset. More...
 
double getEstimatedUsed () const
 Return the estimated duration of the run() function since last reset. More...
 
void getEstimatedUsed (double &av, double &std) const
 Return estimated duration of the run() function since last reset. More...
 
int setPriority (int priority, int policy=-1)
 Set the priority and scheduling policy of the thread, if the OS supports that. More...
 
int getPriority () const
 Query the current priority of the thread, if the OS supports that. More...
 
int getPolicy () const
 Query the current scheduling policy of the thread, if the OS supports that. More...
 
- Public Member Functions inherited from yarp::dev::IThreeAxisGyroscopes
virtual ~IThreeAxisGyroscopes ()
 
- Public Member Functions inherited from yarp::dev::IThreeAxisLinearAccelerometers
virtual ~IThreeAxisLinearAccelerometers ()
 
- Public Member Functions inherited from yarp::dev::IThreeAxisMagnetometers
virtual ~IThreeAxisMagnetometers ()
 
- Public Member Functions inherited from yarp::dev::IOrientationSensors
virtual ~IOrientationSensors ()
 

Additional Inherited Members

- Protected Member Functions inherited from yarp::os::PeriodicThread
virtual void threadRelease ()
 Release method. More...
 
virtual void beforeStart ()
 Called just before a new thread starts. More...
 
virtual void afterStart (bool success)
 Called just after a new thread starts (or fails to start), this is executed by the same thread that calls start(). More...
 

Detailed Description

fakeIMU : fake device implementing the device interface typically implemented by an Inertial Measurement Unit

YARP device name
fakeIMU

The parameters accepted by this device are:

Parameter name SubParameter Type Units Default Value Required Description Notes
period - int millisecond 10 No Period over which the measurement is updated.
constantValue - - - - No If the parameter is present, the fake sensor values never changes (useful for testing server/client coherence).

Definition at line 30 of file fakeIMU.h.

Constructor & Destructor Documentation

◆ fakeIMU() [1/3]

fakeIMU::fakeIMU ( )

This device implements a fake analog sensor emulating an IMU.

Definition at line 37 of file fakeIMU.cpp.

◆ fakeIMU() [2/3]

fakeIMU::fakeIMU ( const fakeIMU )
delete

◆ fakeIMU() [3/3]

fakeIMU::fakeIMU ( fakeIMU &&  )
delete

◆ ~fakeIMU()

fakeIMU::~fakeIMU ( )
override

Definition at line 51 of file fakeIMU.cpp.

Member Function Documentation

◆ calibrate()

bool fakeIMU::calibrate ( int  ch,
double  v 
)
overridevirtual

Calibrate the sensor, single channel.

Parameters
chchannel number
vreset valure
Returns
true/false success/failure

Implements yarp::dev::IGenericSensor.

Definition at line 119 of file fakeIMU.cpp.

◆ close()

bool fakeIMU::close ( )
overridevirtual

Close the DeviceDriver.

Returns
true/false on success/failure.

Reimplemented from yarp::dev::DeviceDriver.

Definition at line 72 of file fakeIMU.cpp.

◆ getChannels()

bool fakeIMU::getChannels ( int *  nc)
overridevirtual

Get the number of channels of the sensor.

Parameters
ncpointer to storage, return value
Returns
true/false success/failure

Implements yarp::dev::IGenericSensor.

Definition at line 113 of file fakeIMU.cpp.

◆ getNrOfOrientationSensors()

size_t fakeIMU::getNrOfOrientationSensors ( ) const
overridevirtual

Get the number of orientation sensors exposed by this device.

Implements yarp::dev::IOrientationSensors.

Definition at line 298 of file fakeIMU.cpp.

◆ getNrOfThreeAxisGyroscopes()

size_t fakeIMU::getNrOfThreeAxisGyroscopes ( ) const
overridevirtual

Get the number of three axis gyroscopes exposed by this sensor.

Implements yarp::dev::IThreeAxisGyroscopes.

Definition at line 184 of file fakeIMU.cpp.

◆ getNrOfThreeAxisLinearAccelerometers()

size_t fakeIMU::getNrOfThreeAxisLinearAccelerometers ( ) const
overridevirtual

Get the number of three axis linear accelerometers exposed by this device.

Implements yarp::dev::IThreeAxisLinearAccelerometers.

Definition at line 222 of file fakeIMU.cpp.

◆ getNrOfThreeAxisMagnetometers()

size_t fakeIMU::getNrOfThreeAxisMagnetometers ( ) const
overridevirtual

Get the number of magnetometers exposed by this device.

Implements yarp::dev::IThreeAxisMagnetometers.

Definition at line 260 of file fakeIMU.cpp.

◆ getOrientationSensorFrameName()

bool fakeIMU::getOrientationSensorFrameName ( size_t  sens_index,
std::string &  frameName 
) const
overridevirtual

Get the name of the frame of the specified sensor.

Note
This is an implementation specific method, that may return the name of the sensor frame in a scenegraph
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IOrientationSensors.

Definition at line 313 of file fakeIMU.cpp.

◆ getOrientationSensorMeasureAsRollPitchYaw()

bool fakeIMU::getOrientationSensorMeasureAsRollPitchYaw ( size_t  sens_index,
yarp::sig::Vector rpy,
double &  timestamp 
) const
overridevirtual

Get the last reading of the orientation sensor as roll pitch yaw.

If $ f $ is the lab or surface fixed frame, and $ s $ is the sensor-fixed frame, this methods returns the angles $ r \in [-180, 180] , p \in [-90, 90], y \in [-180, 180]$ such that

\[ {}^f R_s = RotZ\left(\frac{\pi}{180}y\right)*RotY\left(\frac{\pi}{180}p\right)*RotX\left(\frac{\pi}{180}r\right) \]

with

\[ RotZ(\theta) = \begin{bmatrix} \cos(\theta) & -\sin(\theta) & 0 \\ \sin(\theta) & \cos(\theta) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \]

,

\[ RotY(\theta) = \begin{bmatrix} \cos(\theta) & 0 & \sin(\theta) \\ 0 & 1 & 0 \\ -\sin(\theta) & 0 & \cos(\theta) \\ \end{bmatrix} \]

and

\[ RotX(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\theta) & - \sin(\theta) \\ 0 & \sin(\theta) & \cos(\theta) \\ \end{bmatrix} \]

where $ {}^f R_s \in \mathbb{R}^{3 \times 3} $ is the rotation that left-multiplied by a 3d column vector expressed in $ s $ it returns it expressed in $ f $ .

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfOrientationSensors()-1).
[out]outThe requested measure. The vector should be 3-dimensional. The measure is expressed in degrees .
[out]timestampThe timestamp of the requested measure, expressed in seconds.
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IOrientationSensors.

Definition at line 318 of file fakeIMU.cpp.

◆ getOrientationSensorName()

bool fakeIMU::getOrientationSensorName ( size_t  sens_index,
std::string &  name 
) const
overridevirtual

Get the name of the specified sensor.

Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IOrientationSensors.

Definition at line 308 of file fakeIMU.cpp.

◆ getOrientationSensorStatus()

yarp::dev::MAS_status fakeIMU::getOrientationSensorStatus ( size_t  sens_index) const
overridevirtual

Get the status of the specified sensor.

Implements yarp::dev::IOrientationSensors.

Definition at line 303 of file fakeIMU.cpp.

◆ getThreeAxisGyroscopeFrameName()

bool fakeIMU::getThreeAxisGyroscopeFrameName ( size_t  sens_index,
std::string &  frameName 
) const
overridevirtual

Get the name of the frame of the specified sensor.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisGyroscopes()-1).
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisGyroscopes.

Definition at line 199 of file fakeIMU.cpp.

◆ getThreeAxisGyroscopeMeasure()

bool fakeIMU::getThreeAxisGyroscopeMeasure ( size_t  sens_index,
yarp::sig::Vector out,
double &  timestamp 
) const
overridevirtual

Get the last reading of the gyroscope.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisGyroscopes()-1).
[out]outThe requested measure. The vector should be 3-dimensional. The measure is expressed in degrees/seconds.
[out]timestampThe timestamp of the requested measure, expressed in seconds.
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisGyroscopes.

Definition at line 204 of file fakeIMU.cpp.

◆ getThreeAxisGyroscopeName()

bool fakeIMU::getThreeAxisGyroscopeName ( size_t  sens_index,
std::string &  name 
) const
overridevirtual

Get the name of the specified sensor.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisGyroscopes()-1).
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisGyroscopes.

Definition at line 194 of file fakeIMU.cpp.

◆ getThreeAxisGyroscopeStatus()

yarp::dev::MAS_status fakeIMU::getThreeAxisGyroscopeStatus ( size_t  sens_index) const
overridevirtual

Get the status of the specified sensor.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisGyroscopes()-1).

Implements yarp::dev::IThreeAxisGyroscopes.

Definition at line 189 of file fakeIMU.cpp.

◆ getThreeAxisLinearAccelerometerFrameName()

bool fakeIMU::getThreeAxisLinearAccelerometerFrameName ( size_t  sens_index,
std::string &  frameName 
) const
overridevirtual

Get the name of the frame of the specified sensor.

Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisLinearAccelerometers.

Definition at line 237 of file fakeIMU.cpp.

◆ getThreeAxisLinearAccelerometerMeasure()

bool fakeIMU::getThreeAxisLinearAccelerometerMeasure ( size_t  sens_index,
yarp::sig::Vector out,
double &  timestamp 
) const
overridevirtual

Get the last reading of the specified sensor.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisLinearAccelerometers()-1).
[out]outThe requested measure. The vector should be 3-dimensional. The measure is expressed in meters^2/seconds.
[out]timestampThe timestamp of the requested measure, expressed in seconds.
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisLinearAccelerometers.

Definition at line 242 of file fakeIMU.cpp.

◆ getThreeAxisLinearAccelerometerName()

bool fakeIMU::getThreeAxisLinearAccelerometerName ( size_t  sens_index,
std::string &  name 
) const
overridevirtual

Get the name of the specified sensor.

Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisLinearAccelerometers.

Definition at line 232 of file fakeIMU.cpp.

◆ getThreeAxisLinearAccelerometerStatus()

yarp::dev::MAS_status fakeIMU::getThreeAxisLinearAccelerometerStatus ( size_t  sens_index) const
overridevirtual

Get the status of the specified sensor.

Implements yarp::dev::IThreeAxisLinearAccelerometers.

Definition at line 227 of file fakeIMU.cpp.

◆ getThreeAxisMagnetometerFrameName()

bool fakeIMU::getThreeAxisMagnetometerFrameName ( size_t  sens_index,
std::string &  frameName 
) const
overridevirtual

Get the name of the frame of the specified sensor.

Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisMagnetometers.

Definition at line 275 of file fakeIMU.cpp.

◆ getThreeAxisMagnetometerMeasure()

bool fakeIMU::getThreeAxisMagnetometerMeasure ( size_t  sens_index,
yarp::sig::Vector out,
double &  timestamp 
) const
overridevirtual

Get the last reading of the specified sensor.

Parameters
[in]sens_indexThe index of the specified sensor (should be between 0 and getNrOfThreeAxisMagnetometers()-1).
[out]outThe requested measure. The vector should be 3-dimensional. The measure is expressed in tesla .
[out]timestampThe timestamp of the requested measure, expressed in seconds.
Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisMagnetometers.

Definition at line 280 of file fakeIMU.cpp.

◆ getThreeAxisMagnetometerName()

bool fakeIMU::getThreeAxisMagnetometerName ( size_t  sens_index,
std::string &  name 
) const
overridevirtual

Get the name of the specified sensor.

Returns
false if an error occurred, true otherwise.

Implements yarp::dev::IThreeAxisMagnetometers.

Definition at line 270 of file fakeIMU.cpp.

◆ getThreeAxisMagnetometerStatus()

yarp::dev::MAS_status fakeIMU::getThreeAxisMagnetometerStatus ( size_t  sens_index) const
overridevirtual

Get the status of the specified sensor.

Implements yarp::dev::IThreeAxisMagnetometers.

Definition at line 265 of file fakeIMU.cpp.

◆ open()

bool fakeIMU::open ( yarp::os::Searchable config)
overridevirtual

Open the DeviceDriver.

Parameters
configis a list of parameters for the device. Which parameters are effective for your device can vary. See device invocation examples. If there is no example for your device, you can run the "yarpdev" program with the verbose flag set to probe what parameters the device is checking. If that fails too, you'll need to read the source code (please nag one of the yarp developers to add documentation for your device).
Returns
true/false upon success/failure

Reimplemented from yarp::dev::DeviceDriver.

Definition at line 56 of file fakeIMU.cpp.

◆ operator=() [1/2]

fakeIMU& fakeIMU::operator= ( const fakeIMU )
delete

◆ operator=() [2/2]

fakeIMU& fakeIMU::operator= ( fakeIMU &&  )
delete

◆ read()

bool fakeIMU::read ( yarp::sig::Vector out)
overridevirtual

Read a vector from the sensor.

Parameters
outa vector containing the sensor's last readings.
Returns
true/false success/failure

Implements yarp::dev::IGenericSensor.

Definition at line 78 of file fakeIMU.cpp.


The documentation for this class was generated from the following files: