Class CompassConverter

Class Documentation

class CompassConverter

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().

Public Types

using NodeClockInterface = rclcpp::node_interfaces::NodeClockInterface
using NodeGraphInterface = rclcpp::node_interfaces::NodeGraphInterface
using NodeLoggingInterface = rclcpp::node_interfaces::NodeLoggingInterface
using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface
using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface
using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces<NodeClockInterface, NodeGraphInterface, NodeLoggingInterface, NodeParametersInterface, NodeTopicsInterface>
using Unit = compass_interfaces::msg::Azimuth::_unit_type
using Orientation = compass_interfaces::msg::Azimuth::_orientation_type
using Reference = compass_interfaces::msg::Azimuth::_reference_type
using Variance = compass_interfaces::msg::Azimuth::_variance_type

Public Functions

CompassConverter(rclcpp::Node *node, bool strict)

Create the compass converter.

Parameters:

  • node – [in] The node.

  • strict – [in] Whether to fail if the magnetic model is used outside its natural validity bounds.

CompassConverter(RequiredInterfaces node, bool strict)

Create the compass converter.

Parameters:

  • node – [in] The node.

  • strict – [in] Whether to fail if the magnetic model is used outside its natural validity bounds.

virtual ~CompassConverter()
void configFromParams()

Configure the compass converter from the given ROS parameters.

The parameters read from params interface 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.

  • use_wall_time_for_declination (bool, default false): If set, use wall time for magnetic model queries.

virtual void forceMagneticDeclination(const std::optional<double> &declination)

Force magnetic declination instead of computing it.

Parameters:

declination – [in] The forced declination in rad. If nullopt, declination is set to be computed.

virtual void forceUTMGridConvergence(const std::optional<double> &convergence)

Force UTM grid convergence instead of computing it.

Parameters:

convergence – [in] The forced convergence in rad. If nullopt, convergence is set to be computed.

virtual void setMagneticModelPath(const std::optional<std::string> &modelPath)

Set the path where magnetic models are stored.

Parameters:

modelPath – [in] 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 void forceMagneticModelName(const std::string &model)

Force using the given magnetic model instead of automatically selecting the best one.

Parameters:

model – [in] The name of the model to force (e.g. “wmm2020”). If empty, the best model will be autoselected.

virtual void setUseWallTimeForDeclination(bool use)

If true, magnetic declination computations will use wall time instead of ROS time.

Parameters:

use – [in] Whether to use wall time.

virtual void setKeepUTMZone(bool keep)

Set whether UTM zone should be kept after it is first computed.

Parameters:

keep – [in] If true, the next call to setNavSatPos() will store the computed UTM zone to forcedUTMZone.

virtual void setNavSatPos(const sensor_msgs::msg::NavSatFix &fix)

Callback for GNSS fix (so that the converter can compute UTM grid convergence and store the pose).

Note

This function computes lastUTMGridConvergence if forcedUTMGridConvergence is not set.

Note

This function computes lastUTMZone if forcedUTMGridConvergence is not set.

Note

If keepUTMZone is set and forcedUTMZone is not set, the first determined zone will be remembered in forcedUTMZone.

Parameters:

fix – [in] The fix message. Only latitude, longitude, altitude and header.stamp are relevant.

virtual cras::expected<double, std::string> getMagneticDeclination(const rclcpp::Time &stamp) const

Get the value of magnetic declination for the last location received by setNatSatPos().

Note

If forcedMagneticDeclination is set, it is returned without any adjustments.

Parameters:

stamp – [in] The time for which declination is queried.

Returns:

The magnetic declination in radians or an error message.

virtual cras::expected<double, std::string> computeMagneticDeclination(const sensor_msgs::msg::NavSatFix &fix, const rclcpp::Time &stamp) const

Compute magnetic declination for the given position and time.

Note

This function does not take forcedMagneticDeclination into account.

Parameters:

  • fix – [in] The position to compute declination for.

  • stamp – [in] The time to compute declination for.

Returns:

The magnetic declination in radians or an error message.

virtual cras::expected<double, std::string> getUTMGridConvergence() const

Get the value of UTM grid convergence for the last location received by setNatSatPos().

Note

If forcedUTMGridConvergence is set, it is returned without any adjustments. Otherwise, lastUTMGridConvergence is returned.

Note

If forcedUTMZone is set, the returned grid convergence will be in this zone. Otherwise, it will be in lastUTMZone.

Returns:

The UTM grid convergence in radians, or an error message.

virtual cras::expected<int, std::string> getUTMZone() const

Get the UTM zone of the last location received by setNatSatPos().

Note

If forcedUTMZone is set, it will be directly returned.

Returns:

The UTM zone, or an error message.

virtual void forceUTMZone(const std::optional<int> &zone)

Set the UTM zone that will be used for all future UTM operations.

Note

This function sets forcedUTMZone.

Parameters:

zone – [in] The UTM zone. If nullopt, UTM operations will use the default zone.

virtual cras::expected<std::pair<double, int>, std::string> computeUTMGridConvergenceAndZone(const sensor_msgs::msg::NavSatFix &fix, const std::optional<int> &utmZone) const

Get the value of UTM grid convergence and UTM zone for the provided place.

Note

This function does not take forcedUTMGridConvergence into account.

Parameters:

  • fix – [in] The place for which grid convergence is queried.

  • utmZone – [in] Optional forced UTM zone. If not specified, the default UTM zone will be used.

Returns:

The UTM grid convergence in radians and corresponding UTM zone, or an error message.

virtual cras::expected<compass_interfaces::msg::Azimuth, std::string> convertAzimuth(const compass_interfaces::msg::Azimuth &azimuth, Unit unit, Orientation orientation, Reference reference) const

Convert the given compass_interfaces::msg::Azimuth message parametrized by the given unit, orientation and reference.

Note

To convert between different references, setNavSatPos() has to be called prior to calling this function. The declination and grid convergence of the last set navsat pose will be used.

Parameters:

  • azimuth – [in] The input azimuth message.

  • unit – [in] The output azimuth units.

  • orientation – [in] The output azimuth orientation.

  • reference – [in] The output azimuth reference.

Returns:

The converted Azimuth message or an error message.

virtual cras::expected<compass_interfaces::msg::Azimuth, std::string> convertQuaternion(const geometry_msgs::msg::QuaternionStamped &quat, Variance variance, Unit unit, Orientation orientation, Reference reference) const

Convert the given geometry_msgs::msg::QuaternionStamped message to Azimuth parametrized by the given unit, orientation and reference.

Parameters:

  • quat – [in] The input quaternion message.

  • variance – [in] Variance of the measurement (in rad^2).

  • unit – [in] The output azimuth units.

  • orientation – [in] The output azimuth orientation (this is just a declaration, no conversion is done).

  • reference – [in] The output azimuth reference (this is just a declaration, no conversion is done).

Returns:

The converted Azimuth message or an error message.

virtual cras::expected<compass_interfaces::msg::Azimuth, std::string> convertQuaternion(const geometry_msgs::msg::Quaternion &quat, const std_msgs::msg::Header &header, Variance variance, Unit unit, Orientation orientation, Reference reference) const

Convert the given geometry_msgs::msg::Quaternion message to Azimuth parametrized by the given unit, orientation and reference.

Parameters:

  • quat – [in] The input quaternion message.

  • header – [in] Header of the output message.

  • variance – [in] Variance of the measurement (in rad^2).

  • unit – [in] The output azimuth units.

  • orientation – [in] The output azimuth orientation (this is just a declaration, no conversion is done).

  • reference – [in] The output azimuth reference (this is just a declaration, no conversion is done).

Returns:

The converted Azimuth message or an error message.

cras::expected<compass_interfaces::msg::Azimuth, std::string> convertQuaternionMsgEvent(const std::string &topic, const message_filters::MessageEvent<geometry_msgs::msg::QuaternionStamped const> &quatEvent, Variance variance = 0, Unit unit = compass_interfaces::msg::Azimuth::UNIT_RAD, const std::optional<Orientation> &orientation = {}, const std::optional<Reference> &reference = {}) const

Convert a received geometry_msgs::msg::QuaternionStamped message to Azimuth. If needed, extract the orientation and reference from the topic name.

Parameters:

  • quatEvent – [in] The input quaternion message.

  • variance – [in] Variance of the measurement (in rad^2).

  • unit – [in] The output azimuth units.

  • orientation – [in] The declared input orientation (autodetected from topic if not specified).

  • reference – [in] The declared input reference (autodetected from topic if not specified).

Returns:

The converted Azimuth message or an error message.

cras::expected<compass_interfaces::msg::Azimuth, std::string> convertPoseMsgEvent(const std::string &topic, const message_filters::MessageEvent<geometry_msgs::msg::PoseWithCovarianceStamped const> &poseEvent, Unit unit = compass_interfaces::msg::Azimuth::UNIT_RAD, const std::optional<Orientation> &orientation = {}, const std::optional<Reference> &reference = {}) const

Convert a received geometry_msgs::msg::PoseWithCovarianceStamped message to Azimuth. If needed, extract the orientation and reference from the topic name.

Parameters:

  • poseEvent – [in] The input pose message.

  • unit – [in] The output azimuth units.

  • orientation – [in] The declared input orientation (autodetected from topic if not specified).

  • reference – [in] The declared input reference (autodetected from topic if not specified).

Returns:

The converted Azimuth message or an error message.

cras::expected<compass_interfaces::msg::Azimuth, std::string> convertImuMsgEvent(const std::string &topic, const message_filters::MessageEvent<sensor_msgs::msg::Imu> &imuEvent, Unit unit = compass_interfaces::msg::Azimuth::UNIT_RAD, const std::optional<Orientation> &orientation = {}, const std::optional<Reference> &reference = {}) const

Convert a received sensor_msgs::msg::Imu message to Azimuth. If needed, extract the orientation and reference from the topic name.

Parameters:

  • imuEvent – [in] The input IMU message.

  • unit – [in] The output azimuth units.

  • orientation – [in] The declared input orientation (autodetected from topic if not specified).

  • reference – [in] The declared input reference (autodetected from topic if not specified).

Returns:

The converted Azimuth message or an error message.

cras::expected<compass_interfaces::msg::Azimuth, std::string> convertSerializedMsgEvent(const std::string &topic, const message_filters::MessageEvent<rclcpp::SerializedMessage const> &event, Variance variance = 0, Unit unit = compass_interfaces::msg::Azimuth::UNIT_RAD, const std::optional<Orientation> &orientation = {}, const std::optional<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_interfaces::msg::Azimuth, geometry_msgs::msg::QuaternionStamped, geometry_msgs::msg::PoseWithCovarianceStamped and sensor_msgs::msg::Imu.

Parameters:

  • event – [in] The input message.

  • variance – [in] Variance of the measurement (in rad^2) (if it isn’t a part of the message).

  • unit – [in] The output azimuth units.

  • orientation – [in] The declared input orientation (autodetected from topic if not specified).

  • reference – [in] The declared input reference (autodetected from topic if not specified).

Returns:

The converted Azimuth message or an error message.

virtual cras::expected<geometry_msgs::msg::QuaternionStamped, std::string> convertToQuaternion(const compass_interfaces::msg::Azimuth &azimuth) const

Convert the given Azimuth message to geometry_msgs::msg::QuaternionStamped in the same parametrization.

Parameters:

azimuth – [in] The input azimuth message.

Returns:

The converted geometry_msgs::msg::QuaternionStamped message or an error message.

virtual cras::expected<geometry_msgs::msg::PoseWithCovarianceStamped, std::string> convertToPose(const compass_interfaces::msg::Azimuth &azimuth) const

Convert the given Azimuth message to geometry_msgs::msg::PoseWithCovarianceStamped in the same parametrization.

Parameters:

azimuth – [in] The input azimuth message.

Returns:

The converted geometry_msgs::msg::PoseWithCovarianceStamped message or an error message.

virtual cras::expected<sensor_msgs::msg::Imu, std::string> convertToImu(const compass_interfaces::msg::Azimuth &azimuth) const

Convert the given Azimuth message to sensor_msgs::msg::Imu in the same parametrization.

Parameters:

azimuth – [in] The input azimuth message.

Returns:

The converted sensor_msgs::msg::Imu message or an error message.

Protected Attributes

std::optional<double> lastUTMGridConvergence

UTM convergence of the last received navsat position (or the forced one).

std::optional<int> lastUTMZone

Last determined UTM zone. If empty, no zone has been determined yet.

std::optional<double> forcedMagneticDeclination

The user-forced magnetic declination (if set, do not compute it).

std::optional<double> forcedUTMGridConvergence

The user-forced UTM grid convergence (if set, do not compute it).

std::optional<int> forcedUTMZone

The user-forced UTM zone (if set, do not compute it).

bool keepUTMZone = {true}

If true, the first determined UTM zone will be kept for all future queries.

std::string forcedMagneticModelName = {}

If the user forces a magnetic model, this is its name.

bool useWallTimeForDeclination = {false}

If true, magnetic declination computations will use wall time instead of ROS time.

bool strict = {true}

If true, convertAzimuth() will fail when the magnetic model is used outside its bounds.

std::optional<sensor_msgs::msg::NavSatFix> lastFix

Last received GNSS fix. Used for determining magnetic declination and UTM grid convergence.

RequiredInterfaces node
std::unique_ptr<CompassConverterPrivate> data

PIMPL data.