#include <ImuSensor.hh>
Public Member Functions | |
ImuSensor () | |
constructor | |
virtual | ~ImuSensor () |
destructor | |
math::Vector3d | AngularVelocity () const |
Get the angular velocity of the imu. | |
math::Vector3d | Gravity () const |
Get the gravity vector. | |
virtual bool | HasConnections () const override |
Check if there are any subscribers. | |
virtual bool | Init () override |
Initialize values in the sensor. | |
math::Vector3d | LinearAcceleration () const |
Get the linear acceleration of the imu. | |
virtual bool | Load (const sdf::Sensor &_sdf) override |
Load the sensor based on data from an sdf::Sensor object. | |
virtual bool | Load (sdf::ElementPtr _sdf) override |
Load the sensor with SDF parameters. | |
math::Quaterniond | Orientation () const |
Get the orienation of the imu with respect to reference frame. | |
bool | OrientationEnabled () const |
Get whether or not orientation is enabled. | |
math::Quaterniond | OrientationReference () const |
Get the world orienation reference of the imu. | |
void | SetAngularVelocity (const math::Vector3d &_angularVel) |
Set the angular velocity of the imu. | |
void | SetGravity (const math::Vector3d &_gravity) |
Set the gravity vector. | |
void | SetLinearAcceleration (const math::Vector3d &_linearAcc) |
Set the linear acceleration of the imu. | |
void | SetOrientationEnabled (bool _enabled) |
Set whether to output orientation. Not all imu's generate orientation values as they use filters to produce orientation estimates. | |
void | SetOrientationReference (const math::Quaterniond &_orient) |
Set the orientation reference, i.e. initial imu orientation. Imu orientation data generated will be relative to this reference frame. | |
void | SetWorldFrameOrientation (const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo) |
Specify the rotation offset of the coordinates of the World frame relative to a geo-referenced frame. | |
void | SetWorldPose (const math::Pose3d _pose) |
Set the world pose of the imu. | |
virtual bool | Update (const std::chrono::steady_clock::duration &_now) override |
Update the sensor and generate data. | |
virtual bool | Update (const std::chrono::steady_clock::duration &_now)=0 |
Force the sensor to generate data. | |
bool | Update (const std::chrono::steady_clock::duration &_now, const bool _force) |
Update the sensor. | |
math::Pose3d | WorldPose () const |
Get the world pose of the imu. | |
Public Member Functions inherited from Sensor | |
virtual | ~Sensor () |
destructor | |
void | AddSequence (gz::msgs::Header *_msg, const std::string &_seqKey="default") |
Add a sequence number to a gz::msgs::Header. This function can be called by a sensor that wants to add a sequence number to a sensor message in order to have improved accountability for generated sensor data. | |
bool | EnableMetrics () const |
Get flag state for enabling performance metrics publication. | |
std::string | FrameId () const |
FrameId. | |
bool | HasPendingTrigger () const |
Whether the sensor has a pending trigger. | |
SensorId | Id () const |
Get the sensor's ID. | |
bool | IsActive () const |
Get whether the sensor is enabled or not. | |
bool | IsTriggered () const |
Whether the sensor trigger mode is enabled. | |
std::string | Name () const |
Get name. | |
std::chrono::steady_clock::duration | NextDataUpdateTime () const |
Return the next time the sensor will generate data. | |
std::string | Parent () const |
Get parent link of the sensor. | |
gz::math::Pose3d | Pose () const |
Get the current pose. | |
void | PublishMetrics (const std::chrono::duration< double > &_now) |
Publishes information about the performance of the sensor. This method is called by Update(). | |
sdf::ElementPtr | SDF () const |
Get the SDF used to load this sensor. | |
void | SetActive (bool _active) |
Enable or disable the sensor. Disabled sensors will not generate or publish data unless Update is called with the '_force' argument set to true. | |
void | SetEnableMetrics (bool _enableMetrics) |
Set flag to enable publishing performance metrics. | |
void | SetFrameId (const std::string &_frameId) |
Set Frame ID of the sensor. | |
void | SetNextDataUpdateTime (const std::chrono::steady_clock::duration &_time) |
Manually set the next time the sensor will generate data Useful for accomodating jumps backwards in time as well as specifying updates for non-uniformly updating sensors. | |
virtual void | SetParent (const std::string &_parent) |
Set the parent of the sensor. | |
void | SetPose (const gz::math::Pose3d &_pose) |
Update the pose of the sensor. | |
bool | SetTopic (const std::string &_topic) |
Set topic where sensor data is published. | |
bool | SetTriggered (bool _triggered, const std::string &_triggerTopic="") |
Enable or disable triggered mode. In this mode,. | |
void | SetUpdateRate (const double _hz) |
Set the update rate of the sensor. An update rate of zero means that the sensor is updated every cycle. It's zero by default. | |
std::string | Topic () const |
Get topic where sensor data is published. | |
bool | Update (const std::chrono::steady_clock::duration &_now, const bool _force) |
Update the sensor. | |
double | UpdateRate () const |
Get the update rate of the sensor. | |
Additional Inherited Members | |
Protected Member Functions inherited from Sensor | |
Sensor () | |
constructor | |
Detailed Description
Imu Sensor Class.
An imu sensor that reports linear acceleration, angular velocity, and orientation
Constructor & Destructor Documentation
◆ ImuSensor()
ImuSensor | ( | ) |
constructor
◆ ~ImuSensor()
|
virtual |
destructor
Member Function Documentation
◆ AngularVelocity()
math::Vector3d AngularVelocity | ( | ) | const |
Get the angular velocity of the imu.
- Returns
- Angular velocity of the imu in body frame, expressed in radians per second.
◆ Gravity()
math::Vector3d Gravity | ( | ) | const |
Get the gravity vector.
- Returns
- Gravity vectory in meters per second squared.
◆ HasConnections()
|
overridevirtual |
Check if there are any subscribers.
- Returns
- True if there are subscribers, false otherwise
Reimplemented from Sensor.
◆ Init()
|
overridevirtual |
◆ LinearAcceleration()
math::Vector3d LinearAcceleration | ( | ) | const |
Get the linear acceleration of the imu.
- Returns
- Linear acceleration of the imu in local frame, expressed in meters per second squared.
◆ Load() [1/2]
|
overridevirtual |
◆ Load() [2/2]
|
overridevirtual |
◆ Orientation()
math::Quaterniond Orientation | ( | ) | const |
Get the orienation of the imu with respect to reference frame.
- Returns
- Orientation in reference frame. If orientation is not enabled, this will return the last computed orientation before orientation is disabled or identity Quaternion if orientation has never been enabled.
◆ OrientationEnabled()
bool OrientationEnabled | ( | ) | const |
Get whether or not orientation is enabled.
- Returns
- True if orientation is enabled, false otherwise.
◆ OrientationReference()
math::Quaterniond OrientationReference | ( | ) | const |
Get the world orienation reference of the imu.
- Returns
- Orientation reference in world frame
◆ SetAngularVelocity()
void SetAngularVelocity | ( | const math::Vector3d & | _angularVel | ) |
Set the angular velocity of the imu.
- Parameters
-
[in] _angularVel Angular velocity of the imu in body frame expressed in radians per second
◆ SetGravity()
void SetGravity | ( | const math::Vector3d & | _gravity | ) |
Set the gravity vector.
- Parameters
-
[in] _gravity gravity vector in meters per second squared.
◆ SetLinearAcceleration()
void SetLinearAcceleration | ( | const math::Vector3d & | _linearAcc | ) |
Set the linear acceleration of the imu.
- Parameters
-
[in] _linearAcc Linear accceleration of the imu in body frame expressed in meters per second squared.
◆ SetOrientationEnabled()
void SetOrientationEnabled | ( | bool | _enabled | ) |
Set whether to output orientation. Not all imu's generate orientation values as they use filters to produce orientation estimates.
- Parameters
-
[in] _enabled True to publish orientation data, false to leave the message field empty.
◆ SetOrientationReference()
void SetOrientationReference | ( | const math::Quaterniond & | _orient | ) |
Set the orientation reference, i.e. initial imu orientation. Imu orientation data generated will be relative to this reference frame.
- Parameters
-
[in] _orient Reference orientation
◆ SetWorldFrameOrientation()
void SetWorldFrameOrientation | ( | const math::Quaterniond & | _rot, |
WorldFrameEnumType | _relativeTo | ||
) |
Specify the rotation offset of the coordinates of the World frame relative to a geo-referenced frame.
- Parameters
-
[in] _rot rotation offset [in] _relativeTo world frame orientation, ENU by default
◆ SetWorldPose()
void SetWorldPose | ( | const math::Pose3d | _pose | ) |
Set the world pose of the imu.
- Parameters
-
[in] _pose Pose in world frame
◆ Update() [1/3]
|
overridevirtual |
Update the sensor and generate data.
- Parameters
-
[in] _now The current time
- Returns
- true if the update was successfull
Implements Sensor.
◆ Update() [2/3]
|
virtual |
Force the sensor to generate data.
This method must be overridden by sensors. Subclasses should not not make a decision about whether or not they need to update. The Sensor class will make sure Update() is called at the correct time.
If a subclass wants to have a variable update rate it should call SetUpdateRate().
A subclass should return false if there was an error while updating
- Parameters
-
[in] _now The current time
- Returns
- true if the update was successfull
- See also
- SetUpdateRate()
Implements Sensor.
◆ Update() [3/3]
bool Update | ( | const std::chrono::steady_clock::duration & | _now, |
const bool | _force | ||
) |
Update the sensor.
This is called by the manager, and is responsible for determining if this sensor needs to generate data at this time. If so, the subclasses' Update() method will be called.
- Parameters
-
[in] _now The current time [in] _force Force the update to happen even if it's not time
- Returns
- True if the update was performed (_force was true or _now >= next_update_time or sensor had a pending trigger) and the sensor's bool Sensor::Update(std::chrono::steady_clock::time_point) function returned true. False otherwise.
- Remarks
- If forced the NextUpdateTime() will be unchanged.
◆ WorldPose()
math::Pose3d WorldPose | ( | ) | const |
Get the world pose of the imu.
- Returns
- Pose in world frame.
The documentation for this class was generated from the following file: