compass_converter.h

Go to the documentation of this file.

#pragma once
 
// SPDX-License-Identifier: BSD-3-Clause
// SPDX-FileCopyrightText: Czech Technical University in Prague
 
#include <memory>
#include <string>
#include <utility>
 
#include <compass_msgs/Azimuth.h>
#include <cras_cpp_common/expected.hpp>
#include <cras_cpp_common/log_utils.h>
#include <cras_cpp_common/optional.hpp>
#include <cras_cpp_common/param_utils/bound_param_helper.hpp>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/message_event.h>
#include <ros/time.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Header.h>
#include <topic_tools/shape_shifter.h>
 
namespace compass_conversions
{
 
struct CompassConverterPrivate;
 
class CompassConverter : public cras::HasLogger
{
public:
  CompassConverter(const cras::LogHelperPtr& log, bool strict);
  virtual ~CompassConverter();
 
  void configFromParams(const cras::BoundParamHelper& params);
 
  virtual void forceMagneticDeclination(const cras::optional<double>& declination);
 
  virtual void forceUTMGridConvergence(const cras::optional<double>& convergence);
 
  virtual void setMagneticModelPath(const cras::optional<std::string>& modelPath);
 
  virtual void forceMagneticModelName(const std::string& model);
 
  virtual void setKeepUTMZone(bool keep);
 
  virtual void setNavSatPos(const sensor_msgs::NavSatFix& fix);
 
  virtual cras::expected<double, std::string> getMagneticDeclination(const ros::Time& stamp) const;
 
  virtual cras::expected<double, std::string> computeMagneticDeclination(
    const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const;
 
  virtual cras::expected<double, std::string> getUTMGridConvergence() const;
 
  virtual cras::expected<int, std::string> getUTMZone() const;
 
  virtual void forceUTMZone(const cras::optional<int>& zone);
 
  virtual cras::expected<std::pair<double, int>, std::string> computeUTMGridConvergenceAndZone(
    const sensor_msgs::NavSatFix& fix, const cras::optional<int>& utmZone) const;
 
  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;
 
  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;
 
  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;
 
  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;
 
  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;
 
  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;
 
  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;
 
  virtual cras::expected<geometry_msgs::QuaternionStamped, std::string> convertToQuaternion(
    const compass_msgs::Azimuth& azimuth) const;
 
  virtual cras::expected<geometry_msgs::PoseWithCovarianceStamped, std::string> convertToPose(
    const compass_msgs::Azimuth& azimuth) const;
 
  virtual cras::expected<sensor_msgs::Imu, std::string> convertToImu(const compass_msgs::Azimuth& azimuth) const;
 
protected:
  cras::optional<double> lastUTMGridConvergence;
 
  cras::optional<int> lastUTMZone;
 
  cras::optional<double> forcedMagneticDeclination;
 
  cras::optional<double> forcedUTMGridConvergence;
 
  cras::optional<int> forcedUTMZone;
 
  bool keepUTMZone {true};
 
  std::string forcedMagneticModelName{};
 
  bool strict {true};
 
  cras::optional<sensor_msgs::NavSatFix> lastFix;
 
  std::unique_ptr<CompassConverterPrivate> data;
};
 
}