Convert between various compass representations. More...
#include <compass_converter.h>
Public Member Functions | |
CompassConverter (const cras::LogHelperPtr &log, bool strict) | |
Create the compass converter. More... | |
virtual cras::expected< double, std::string > | computeMagneticDeclination (const sensor_msgs::NavSatFix &fix, const ros::Time &stamp) const |
Compute magnetic declination for the given position and time. More... | |
virtual cras::expected< std::pair< double, int >, std::string > | computeUTMGridConvergenceAndZone (const sensor_msgs::NavSatFix &fix, const cras::optional< int > &utmZone) const |
Get the value of UTM grid convergence and UTM zone for the provided place. More... | |
void | configFromParams (const cras::BoundParamHelper ¶ms) |
Configure the compass converter from the given ROS parameters. More... | |
virtual cras::expected< compass_msgs::Azimuth, std::string > | convertAzimuth (const compass_msgs::Azimuth &azimuth, decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, decltype(compass_msgs::Azimuth::reference) reference) const |
Convert the given compass_msgs::Azimuth message parametrized by the given unit, orientation and reference. More... | |
cras::expected< compass_msgs::Azimuth, std::string > | convertImuMsgEvent (const ros::MessageEvent< sensor_msgs::Imu const > &imuEvent, decltype(compass_msgs::Azimuth::unit) unit=compass_msgs::Azimuth::UNIT_RAD, const cras::optional< decltype(compass_msgs::Azimuth::orientation)> &orientation={}, const cras::optional< decltype(compass_msgs::Azimuth::reference)> &reference={}) const |
Convert a received sensor_msgs::Imu message to Azimuth. If needed, extract the orientation and reference from the topic name. More... | |
cras::expected< compass_msgs::Azimuth, std::string > | convertPoseMsgEvent (const ros::MessageEvent< geometry_msgs::PoseWithCovarianceStamped const > &poseEvent, decltype(compass_msgs::Azimuth::unit) unit=compass_msgs::Azimuth::UNIT_RAD, const cras::optional< decltype(compass_msgs::Azimuth::orientation)> &orientation={}, const cras::optional< decltype(compass_msgs::Azimuth::reference)> &reference={}) const |
Convert a received geometry_msgs::PoseWithCovarianceStamped message to Azimuth. If needed, extract the orientation and reference from the topic name. More... | |
virtual cras::expected< compass_msgs::Azimuth, std::string > | convertQuaternion (const geometry_msgs::Quaternion &quat, const std_msgs::Header &header, decltype(compass_msgs::Azimuth::variance) variance, decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, decltype(compass_msgs::Azimuth::reference) reference) const |
Convert the given geometry_msgs::Quaternion message to Azimuth parametrized by the given unit, orientation and reference. More... | |
virtual cras::expected< compass_msgs::Azimuth, std::string > | convertQuaternion (const geometry_msgs::QuaternionStamped &quat, decltype(compass_msgs::Azimuth::variance) variance, decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, decltype(compass_msgs::Azimuth::reference) reference) const |
Convert the given geometry_msgs::QuaternionStamped message to Azimuth parametrized by the given unit, orientation and reference. More... | |
cras::expected< compass_msgs::Azimuth, std::string > | convertQuaternionMsgEvent (const ros::MessageEvent< geometry_msgs::QuaternionStamped const > &quatEvent, decltype(compass_msgs::Azimuth::variance) variance=0, decltype(compass_msgs::Azimuth::unit) unit=compass_msgs::Azimuth::UNIT_RAD, const cras::optional< decltype(compass_msgs::Azimuth::orientation)> &orientation={}, const cras::optional< decltype(compass_msgs::Azimuth::reference)> &reference={}) const |
Convert a received geometry_msgs::QuaternionStamped message to Azimuth. If needed, extract the orientation and reference from the topic name. More... | |
virtual cras::expected< sensor_msgs::Imu, std::string > | convertToImu (const compass_msgs::Azimuth &azimuth) const |
Convert the given Azimuth message to sensor_msgs::Imu in the same parametrization. More... | |
virtual cras::expected< geometry_msgs::PoseWithCovarianceStamped, std::string > | convertToPose (const compass_msgs::Azimuth &azimuth) const |
Convert the given Azimuth message to geometry_msgs::PoseWithCovarianceStamped in the same parametrization. More... | |
virtual cras::expected< geometry_msgs::QuaternionStamped, std::string > | convertToQuaternion (const compass_msgs::Azimuth &azimuth) const |
Convert the given Azimuth message to geometry_msgs::QuaternionStamped in the same parametrization. More... | |
cras::expected< compass_msgs::Azimuth, std::string > | convertUniversalMsgEvent (const ros::MessageEvent< topic_tools::ShapeShifter const > &event, decltype(compass_msgs::Azimuth::variance) variance=0, decltype(compass_msgs::Azimuth::unit) unit=compass_msgs::Azimuth::UNIT_RAD, const cras::optional< decltype(compass_msgs::Azimuth::orientation)> &orientation={}, const cras::optional< decltype(compass_msgs::Azimuth::reference)> &reference={}) const |
Convert a received message to Azimuth. If needed, extract the orientation and reference from the topic name. More... | |
virtual void | forceMagneticDeclination (const cras::optional< double > &declination) |
Force magnetic declination instead of computing it. More... | |
virtual void | forceMagneticModelName (const std::string &model) |
Force using the given magnetic model instead of automatically selecting the best one. More... | |
virtual void | forceUTMGridConvergence (const cras::optional< double > &convergence) |
Force UTM grid convergence instead of computing it. More... | |
virtual void | forceUTMZone (const cras::optional< int > &zone) |
Set the UTM zone that will be used for all future UTM operations. More... | |
virtual cras::expected< double, std::string > | getMagneticDeclination (const ros::Time &stamp) const |
Get the value of magnetic declination for the last location received by setNatSatPos(). More... | |
virtual cras::expected< double, std::string > | getUTMGridConvergence () const |
Get the value of UTM grid convergence for the last location received by setNatSatPos(). More... | |
virtual cras::expected< int, std::string > | getUTMZone () const |
Get the UTM zone of the last location received by setNatSatPos(). More... | |
virtual void | setKeepUTMZone (bool keep) |
Set whether UTM zone should be kept after it is first computed. More... | |
virtual void | setMagneticModelPath (const cras::optional< std::string > &modelPath) |
Set the path where magnetic models are stored. More... | |
virtual void | setNavSatPos (const sensor_msgs::NavSatFix &fix) |
Callback for GNSS fix (so that the converter can compute UTM grid convergence and store the pose). More... | |
virtual | ~CompassConverter () |
Public Member Functions inherited from cras::HasLogger | |
::cras::LogHelperConstPtr | getCrasLogger () const |
HasLogger (const ::cras::LogHelperPtr &log) | |
void | setCrasLogger (const ::cras::LogHelperPtr &log) |
Protected Attributes | |
std::unique_ptr< CompassConverterPrivate > | data |
PIMPL data. More... | |
cras::optional< double > | forcedMagneticDeclination |
The user-forced magnetic declination (if set, do not compute it). More... | |
std::string | forcedMagneticModelName {} |
If the user forces a magnetic model, this is its name. More... | |
cras::optional< double > | forcedUTMGridConvergence |
The user-forced UTM grid convergence (if set, do not compute it). More... | |
cras::optional< int > | forcedUTMZone |
The user-forced UTM zone (if set, do not compute it). More... | |
bool | keepUTMZone {true} |
If true, the first determined UTM zone will be kept for all future queries. More... | |
cras::optional< sensor_msgs::NavSatFix > | lastFix |
Last received GNSS fix. Used for determining magnetic declination and UTM grid convergence. More... | |
cras::optional< double > | lastUTMGridConvergence |
UTM convergence of the last received navsat position (or the forced one). More... | |
cras::optional< int > | lastUTMZone |
Last determined UTM zone. If empty, no zone has been determined yet. More... | |
bool | strict {true} |
If true, convertAzimuth() will fail when the magnetic model is used outside its bounds. More... | |
Protected Attributes inherited from cras::HasLogger | |
::cras::LogHelperPtr | log |
Convert between various compass representations.
When converting only units or orientations, no NavSatFix is needed. However, if you need to convert between different references, you need to provide a NavSatFix with valid GNSS position and time and set it using setNavSatPos().
Definition at line 43 of file compass_converter.h.
compass_conversions::CompassConverter::CompassConverter | ( | const cras::LogHelperPtr & | log, |
bool | strict | ||
) |
Create the compass converter.
[in] | log | The logger. |
[in] | strict | Whether to fail if the magnetic model is used outside its natural validity bounds. |
|
virtual |
|
virtual |
Compute magnetic declination for the given position and time.
[in] | fix | The position to compute declination for. |
[in] | stamp | The time to compute declination for. |
|
virtual |
Get the value of UTM grid convergence and UTM zone for the provided place.
[in] | fix | The place for which grid convergence is queried. |
[in] | utmZone | Optional forced UTM zone. If not specified, the default UTM zone will be used. |
void compass_conversions::CompassConverter::configFromParams | ( | const cras::BoundParamHelper & | params | ) |
Configure the compass converter from the given ROS parameters.
[in] | params | ROS parameters that configure the converter. |
The parameters read from params struct are:
magnetic_declination
(double, radians, optional): If set, forces this value of magnetic declination.utm_grid_convergence
(double, radians, optional): If set, forces this value of UTM grid convergence.magnetic_models_path
(string, default "$PACKAGE/data/magnetic"): Path where WMM magnetic models can be found. If set to empty string, the models will be searched in a default folder of GeographicLib. Environment variables GEOGRAPHICLIB_MAGNETIC_PATH
or GEOGRAPHICLIB_DATA
influence the location of this folder.magnetic_model
(string, optional): If set, forces using the given WMM model instead of determining the proper one by year. Example value is "wmm2020".utm_zone
(int, optional): If set, forces using this UTM zone instead of determining the proper one.keep_utm_zone
(bool, default true): If true, the first determined UTM zone will be used for all future conversions.initial_lat
(double, degrees, optional): If set, use this latitude before the first navsat pose is received.initial_lon
(double, degrees, optional): If set, use this longitude before the first navsat pose is received.initial_alt
(double, meters, optional): If set, use this altitude before the first navsat pose is received.
|
virtual |
Convert the given compass_msgs::Azimuth message parametrized by the given unit, orientation and reference.
[in] | azimuth | The input azimuth message. |
[in] | unit | The output azimuth units. |
[in] | orientation | The output azimuth orientation. |
[in] | reference | The output azimuth reference. |
cras::expected<compass_msgs::Azimuth, std::string> compass_conversions::CompassConverter::convertImuMsgEvent | ( | const ros::MessageEvent< sensor_msgs::Imu const > & | imuEvent, |
decltype(compass_msgs::Azimuth::unit) | unit = compass_msgs::Azimuth::UNIT_RAD , |
||
const cras::optional< decltype(compass_msgs::Azimuth::orientation)> & | orientation = {} , |
||
const cras::optional< decltype(compass_msgs::Azimuth::reference)> & | reference = {} |
||
) | const |
Convert a received sensor_msgs::Imu message to Azimuth. If needed, extract the orientation and reference from the topic name.
[in] | imuEvent | The input IMU message. |
[in] | unit | The output azimuth units. |
[in] | orientation | The declared input orientation (autodetected from topic if not specified). |
[in] | reference | The declared input reference (autodetected from topic if not specified). |
cras::expected<compass_msgs::Azimuth, std::string> compass_conversions::CompassConverter::convertPoseMsgEvent | ( | const ros::MessageEvent< geometry_msgs::PoseWithCovarianceStamped const > & | poseEvent, |
decltype(compass_msgs::Azimuth::unit) | unit = compass_msgs::Azimuth::UNIT_RAD , |
||
const cras::optional< decltype(compass_msgs::Azimuth::orientation)> & | orientation = {} , |
||
const cras::optional< decltype(compass_msgs::Azimuth::reference)> & | reference = {} |
||
) | const |
Convert a received geometry_msgs::PoseWithCovarianceStamped message to Azimuth. If needed, extract the orientation and reference from the topic name.
[in] | poseEvent | The input pose message. |
[in] | unit | The output azimuth units. |
[in] | orientation | The declared input orientation (autodetected from topic if not specified). |
[in] | reference | The declared input reference (autodetected from topic if not specified). |
|
virtual |
Convert the given geometry_msgs::Quaternion message to Azimuth parametrized by the given unit, orientation and reference.
[in] | quat | The input quaternion message. |
[in] | header | Header of the output message. |
[in] | variance | Variance of the measurement (in rad^2). |
[in] | unit | The output azimuth units. |
[in] | orientation | The output azimuth orientation (this is just a declaration, no conversion is done). |
[in] | reference | The output azimuth reference (this is just a declaration, no conversion is done). |
|
virtual |
Convert the given geometry_msgs::QuaternionStamped message to Azimuth parametrized by the given unit, orientation and reference.
[in] | quat | The input quaternion message. |
[in] | variance | Variance of the measurement (in rad^2). |
[in] | unit | The output azimuth units. |
[in] | orientation | The output azimuth orientation (this is just a declaration, no conversion is done). |
[in] | reference | The output azimuth reference (this is just a declaration, no conversion is done). |
cras::expected<compass_msgs::Azimuth, std::string> compass_conversions::CompassConverter::convertQuaternionMsgEvent | ( | const ros::MessageEvent< geometry_msgs::QuaternionStamped const > & | quatEvent, |
decltype(compass_msgs::Azimuth::variance) | variance = 0 , |
||
decltype(compass_msgs::Azimuth::unit) | unit = compass_msgs::Azimuth::UNIT_RAD , |
||
const cras::optional< decltype(compass_msgs::Azimuth::orientation)> & | orientation = {} , |
||
const cras::optional< decltype(compass_msgs::Azimuth::reference)> & | reference = {} |
||
) | const |
Convert a received geometry_msgs::QuaternionStamped message to Azimuth. If needed, extract the orientation and reference from the topic name.
[in] | quatEvent | The input quaternion message. |
[in] | variance | Variance of the measurement (in rad^2). |
[in] | unit | The output azimuth units. |
[in] | orientation | The declared input orientation (autodetected from topic if not specified). |
[in] | reference | The declared input reference (autodetected from topic if not specified). |
|
virtual |
Convert the given Azimuth message to sensor_msgs::Imu in the same parametrization.
[in] | azimuth | The input azimuth message. |
|
virtual |
Convert the given Azimuth message to geometry_msgs::PoseWithCovarianceStamped in the same parametrization.
[in] | azimuth | The input azimuth message. |
|
virtual |
Convert the given Azimuth message to geometry_msgs::QuaternionStamped in the same parametrization.
[in] | azimuth | The input azimuth message. |
cras::expected<compass_msgs::Azimuth, std::string> compass_conversions::CompassConverter::convertUniversalMsgEvent | ( | const ros::MessageEvent< topic_tools::ShapeShifter const > & | event, |
decltype(compass_msgs::Azimuth::variance) | variance = 0 , |
||
decltype(compass_msgs::Azimuth::unit) | unit = compass_msgs::Azimuth::UNIT_RAD , |
||
const cras::optional< decltype(compass_msgs::Azimuth::orientation)> & | orientation = {} , |
||
const cras::optional< decltype(compass_msgs::Azimuth::reference)> & | reference = {} |
||
) | const |
Convert a received message to Azimuth. If needed, extract the orientation and reference from the topic name.
Supported message types are compass_msgs::Azimuth, geometry_msgs::QuaternionStamped, geometry_msgs::PoseWithCovarianceStamped and sensor_msgs::Imu.
[in] | event | The input message. |
[in] | variance | Variance of the measurement (in rad^2) (if it isn't a part of the message). |
[in] | unit | The output azimuth units. |
[in] | orientation | The declared input orientation (autodetected from topic if not specified). |
[in] | reference | The declared input reference (autodetected from topic if not specified). |
|
virtual |
Force magnetic declination instead of computing it.
[in] | declination | The forced declination in rad. If nullopt, declination is set to be computed. |
|
virtual |
Force using the given magnetic model instead of automatically selecting the best one.
[in] | model | The name of the model to force (e.g. "wmm2020"). If empty, the best model will be autoselected. |
|
virtual |
Force UTM grid convergence instead of computing it.
[in] | convergence | The forced convergence in rad. If nullopt, convergence is set to be computed. |
|
virtual |
Set the UTM zone that will be used for all future UTM operations.
[in] | zone | The UTM zone. If nullopt, UTM operations will use the default zone. |
|
virtual |
Get the value of magnetic declination for the last location received by setNatSatPos().
[in] | stamp | The time for which declination is queried. |
|
virtual |
Get the value of UTM grid convergence for the last location received by setNatSatPos().
|
virtual |
Get the UTM zone of the last location received by setNatSatPos().
|
virtual |
Set whether UTM zone should be kept after it is first computed.
[in] | keep | If true, the next call to setNavSatPos() will store the computed UTM zone to forcedUTMZone. |
|
virtual |
Set the path where magnetic models are stored.
[in] | modelPath | Path to the folder with stored models. If nullopt, the default data distributed with this package will be used. If empty string, a default system location will be used. The default system location is determined by GeographicLib and can be influenced by setting environment variables GEOGRAPHICLIB_MAGNETIC_PATH or GEOGRAPHICLIB_DATA . |
|
virtual |
Callback for GNSS fix (so that the converter can compute UTM grid convergence and store the pose).
[in] | fix | The fix message. Only latitude , longitude , altitude and header.stamp are relevant. |
|
protected |
PIMPL data.
Definition at line 353 of file compass_converter.h.
|
protected |
The user-forced magnetic declination (if set, do not compute it).
Definition at line 332 of file compass_converter.h.
|
protected |
If the user forces a magnetic model, this is its name.
Definition at line 344 of file compass_converter.h.
|
protected |
The user-forced UTM grid convergence (if set, do not compute it).
Definition at line 335 of file compass_converter.h.
|
protected |
The user-forced UTM zone (if set, do not compute it).
Definition at line 338 of file compass_converter.h.
|
protected |
If true, the first determined UTM zone will be kept for all future queries.
Definition at line 341 of file compass_converter.h.
|
protected |
Last received GNSS fix. Used for determining magnetic declination and UTM grid convergence.
Definition at line 350 of file compass_converter.h.
|
protected |
UTM convergence of the last received navsat position (or the forced one).
Definition at line 326 of file compass_converter.h.
|
protected |
Last determined UTM zone. If empty, no zone has been determined yet.
Definition at line 329 of file compass_converter.h.
|
protected |
If true, convertAzimuth() will fail when the magnetic model is used outside its bounds.
Definition at line 347 of file compass_converter.h.