diff --git a/.github/ci.rosinstall b/.github/ci.rosinstall new file mode 100644 index 0000000..10371ca --- /dev/null +++ b/.github/ci.rosinstall @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +- git: + local-name: ros-utils + uri: https://github.com/ctu-vras/ros-utils.git + version: master \ No newline at end of file diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..19e6e17 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,37 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: CI + +# This determines when this workflow is run +on: [push, pull_request] # on all pushes and PRs + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: testing} + env: + CCACHE_DIR: ${{ github.workspace }}/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) + UPSTREAM_WORKSPACE: .github/ci.rosinstall + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + # This step will fetch/store the directory used by ccache before/after the ci run + - name: Cache ccache + uses: rhaschke/cache@main + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}-${{ github.sha }} + ccache-${{ matrix.env.ROS_DISTRO }} + env: + GHA_CACHE_SAVE: always + # Run industrial_ci + - uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} diff --git a/compass_msgs/LICENSE b/LICENSES/BSD-3-Clause.txt similarity index 100% rename from compass_msgs/LICENSE rename to LICENSES/BSD-3-Clause.txt diff --git a/LICENSES/LicenseRef-WMM-Public-Domain.txt b/LICENSES/LicenseRef-WMM-Public-Domain.txt new file mode 100644 index 0000000..0e6aad8 --- /dev/null +++ b/LICENSES/LicenseRef-WMM-Public-Domain.txt @@ -0,0 +1 @@ +The WMM source code is in the public domain and not licensed or under copyright. The information and software may be used freely by the public. As required by 17 U.S.C. 403, third parties producing copyrighted works consisting predominantly of the material produced by U.S. government agencies must provide notice with such work(s) identifying the U.S. Government material incorporated and stating that such material is not subject to copyright protection. \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..5cf74e8 --- /dev/null +++ b/README.md @@ -0,0 +1,50 @@ + + + +# Compass stack + +This collection of packages provides support for working with azimuths in ROS. + +## Packages + +- [compass_msgs](compass_msgs): The message definitions. +- [compass_conversions](compass_conversions): Helpers for converting between different representations of azimuths. +- [magnetic_model](magnetic_model): ROS bindings for World Magnetic Model. +- [magnetometer_compass](magnetometer_compass): Support and ROS nodes for extracting azimuths from 3-axis magnetometers. +- [magnetometer_pipeline](magnetometer_pipeline): Calibration and removing of magnetometer bias. + +## Definitions + +**ENU** frame is the [standard orientation used in ROS](https://www.ros.org/reps/rep-0103.html). The abbreviation means +East-North-Up and corresponds to the meaning of vector components X, Y and Z. A zero azimuth points towards East and it +increases counter-clockwise. + +**NED** frame is the "intuitive" North-East-Down orientation where the zero azimuth points to North and increases +clockwise, just as you are used to when using a compass. + +**Magnetic azimuth** is the angle between Earth's magnetic North (or East in ENU frame) and a specified direction. + +**True azimuth** (also called geographic, map or geodetic North) is the angle between Earth's geographic North (or East +in ENU frame) and a specified direction. + +**UTM azimuth** (also called grid azimuth) is the angle between UTM North (or East in ENU frame) and a specified +direction. [UTM](https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system) is a planar projection +of Earth's surface onto predefined rectangles, which yields a Cartesian coordinate system. The Earth is divided into +several stripes which are unrolled into a plane to form the UTM grid. These stripes are called **UTM zones**. Each UTM +zone is 6 degrees of longitude wide, but it is considered valid in a slightly larger area, approximately 100 km +outside its precise bounds. This allows sticking to a single UTM zone to prevent zone switching when moving close to +the boundary of two zones. + +The difference between magnetic and true North is called **magnetic declination**. Its values are location- and +time-dependent and they are approximated by the +[World Magnetic Model](https://www.ncei.noaa.gov/products/world-magnetic-model). + +The difference between true North and grid North is called **grid convergence**. Its values are only location-dependent +and do not differ in time. The values also depend on the chosen UTM zone. + +Although [ROS specifies that all angular values should be expressed in radians](https://www.ros.org/reps/rep-0103.html), +the usage of degrees in geography is so common that +[Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html) messages support both radians and degrees. + +For more information, see https://www.drillingformulas.com/magnetic-declination-and-grid-convergent-and-their-applications-in-directional-drilling/ +or https://en.wikipedia.org/wiki/Azimuth . diff --git a/compass_conversions/CMakeLists.txt b/compass_conversions/CMakeLists.txt new file mode 100644 index 0000000..4aaaabf --- /dev/null +++ b/compass_conversions/CMakeLists.txt @@ -0,0 +1,116 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +cmake_minimum_required(VERSION 3.10.2) +project(compass_conversions) + +set(CMAKE_CXX_STANDARD 17) + +find_package(catkin REQUIRED COMPONENTS + angles + compass_msgs + cras_cpp_common + geometry_msgs + magnetic_model + message_filters + nodelet + pluginlib + roscpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + topic_tools +) + +# Ubuntu libgeographic-dev package installs into non-standard location +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES compass_converter compass_message_filter compass_topic_names magnetic_model tf2_compass_msgs + CATKIN_DEPENDS compass_msgs cras_cpp_common geometry_msgs message_filters roscpp sensor_msgs std_msgs tf2 topic_tools +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} +) + +add_library(compass_topic_names src/topic_names.cpp) +target_link_libraries(compass_topic_names ${catkin_LIBRARIES}) + +add_library(compass_converter src/compass_converter.cpp) +add_dependencies(compass_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(compass_converter PUBLIC ${catkin_LIBRARIES} PRIVATE compass_topic_names ${GeographicLib_LIBRARIES}) + +add_library(compass_message_filter src/message_filter.cpp) +target_link_libraries(compass_message_filter compass_converter) + +add_library(tf2_compass_msgs src/tf2_compass_msgs.cpp) +target_link_libraries(tf2_compass_msgs ${catkin_LIBRARIES}) + +add_library(compass_transformer_nodelet nodelets/compass_transformer.cpp) +target_link_libraries(compass_transformer_nodelet compass_message_filter compass_topic_names tf2_compass_msgs ${catkin_LIBRARIES}) +cras_node_from_nodelet(compass_transformer_nodelet ${PROJECT_NAME}::CompassTransformerNodelet OUTPUT_NAME compass_transformer ANONYMOUS) + +install(TARGETS compass_converter compass_message_filter compass_topic_names compass_transformer_nodelet tf2_compass_msgs + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + find_package(rostest REQUIRED) + + roslint_custom(catkin_lint "-W2" .) + + # Roslint C++ - checks formatting and some other rules for C++ files + + file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h) + file(GLOB_RECURSE ROSLINT_SRC src/*.cpp nodelets/*.cpp) + file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) + + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-build/include,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright") + roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC} ${ROSLINT_TEST}) + + roslint_add_test() + + catkin_add_gtest(test_compass_converter test/test_compass_converter.cpp) + target_link_libraries(test_compass_converter compass_converter) + + catkin_add_gtest(test_message_filter test/test_message_filter.cpp) + target_link_libraries(test_message_filter compass_message_filter) + + catkin_add_gtest(test_tf2_compass_msgs test/test_tf2_compass_msgs.cpp) + target_link_libraries(test_tf2_compass_msgs tf2_compass_msgs) + + catkin_add_gtest(test_compass_topic_names test/test_topic_names.cpp) + target_link_libraries(test_compass_topic_names compass_topic_names) + + add_rostest_gtest(test_compass_transformer_nodelet test/test_compass_transformer_nodelet.test test/test_compass_transformer_nodelet.cpp) + target_link_libraries(test_compass_transformer_nodelet ${catkin_LIBRARIES} compass_transformer_nodelet) + if(CMAKE_VERSION VERSION_LESS "3.13.0") + set_property(TARGET test_compass_transformer_nodelet APPEND_STRING PROPERTY LINK_FLAGS " -Wl,--no-as-needed") + else() + #catkin_lint: ignore_once cmake_old + target_link_options(test_compass_transformer_nodelet PUBLIC "LINKER:--no-as-needed") + endif() +endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/compass_conversions/README.md b/compass_conversions/README.md new file mode 100644 index 0000000..77c4c9a --- /dev/null +++ b/compass_conversions/README.md @@ -0,0 +1,138 @@ + + + +# compass\_conversions + +This package contains utilities for converting between the various parametrizations of +[Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html) messages. + +See [readme of the compass stack](../README.md) for definitions of the terms used in this readme. + +## C++ Libraries + +### compass\_conversions::CompassConverter + +Helper for direct conversions between parametrizations. + +Conversions between different references (e.g. Mag North to True North) require a +[NavSatFix](https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatFix.html) with the corresponding absolute pose. + +Conversions involving UTM can receive a [Int32](https://docs.ros.org/en/api/std_msgs/html/msg/Int32.html) parameter +that forces the use of a specific UTM zone instead of the default one. + +The converter accepts several parameters that modify its behavior: + +- `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. +- `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 automatically 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. + +### compass\_conversions::UniversalAzimuthSubscriber + +A [message filter](https://wiki.ros.org/message_filters) `Subscriber` that subscribes any of the supported azimuth +representations ( +[Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html), +[QuaternionsStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/QuaternionStamped.html), +[PoseWithCovarianceStamped](https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html), +[Imu](https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html), +) and converts it to [Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html). + +Some parameters might be needed for the conversion, and you can either supply them as arguments of the Subscriber, +or they can be autodetected from the topic name (if it follows the output of topic names module in this package). + +### compass\_conversions::CompassFilter + +A [message filter](https://wiki.ros.org/message_filters) that converts incoming +[Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html) messages to a specified parametrization. +It can also handle a second input with [NavSatFix](https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatFix.html) +messages to allow converting between different references. It also handles a third input with +[Int32](https://docs.ros.org/en/api/std_msgs/html/msg/Int32.html) messages to allow forcing a specific UTM zone. + +Example usage: + +``` +message_filters::Subscriber azimuthInput(...); +message_filters::Subscriber fixInput(...); +compass_conversions::CompassFilter filter(log, nullptr, azimuthInput, fixInput, + compass_msgs::Azimuth::UNIT_RAD, compass_msgs::Azimuth::ORIENTATION_ENU, + compass_msgs::Azimuth::REFERENCE_GEOGRAPHIC); +filter.registerCallback([](const compass_msgs::AzimuthConstPtr& msg) { + ... // Handle the data +}); +``` + +### compass\_conversions/tf2\_compass\_msgs.h + +When you include this file, `tf2::convert()` and similar functions will gain the ability to transform +[Azimuth](https://docs.ros.org/en/api/compass_msgs/html/msg/Azimuth.html) messages. + +### compass\_conversions/topic\_names.h + +Functions that assign topic names (or suffixes) to various parametrizations and representations of azimuths. +These unique topic name suffixes can be used to add important metadata to messages that do not carry them inside. + +## Nodelets + +### compass\_conversions/compass\_transformer + +This nodelet subscribes to incoming azimuth messages (using the `UniversalAzimuthSubscriber` described above), +transforms them into a different parametrization (using `CompassFilter`), transforms them into a different TF frame +(using `tf2_compass_msgs.h`) and publishes them in the desired parametrization and representation (possibly using +`CompassConverter` to do the conversion). + +The nodelet can also be launched as a standalone node using `rosrun compass_conversions compass_transformer`. + +#### Subscribed topics: + +- `~azimuth_in` (compass_msgs/Azimuth or geometry_msgs/QuaternionStamped or geometry_msgs/PoseWithCovarianceStamped + or sensor_msgs/Imu): The input azimuth. The name of the topic (if you remap it) can be used to autodetect some + metadata for the conversion. + - `fix` (sensor_msgs/NavSatFix): GNSS fix messages that can be used to determine some parameters for the conversion. + - `utm_zone` (std_msgs/Int32): Optional messages with forced UTM zone. + - TF (only if `~target_frame` is nonempty) + +#### Published topics: + + - `~azimuth_out` or `~azimuth_out/SUFFIX`: The transformed azimuth. If `~target_append_suffix` is true, the variant + with topic name suffix will be used (e.g. `~azimuth_out/mag/enu/deg`). + The type of the published message is determined by `~target_type`. + +#### Parameters: + +- `~queue_size` (int, default 10): Queue size for the subscribers and publishers. +- `~target_unit` (str, 'deg' or 'rad', default: no change): Angular unit to be used in the transformed messages. +- `~target_orientation` (str, 'enu' or 'ned', default: no change): ENU or NED orientation to be used in the + transformed messages. +- `~target_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used in the + transformed messages. +- `~target_type` (str, 'azimuth', 'quaternion', 'pose' or 'imu', default 'azimuth'): The Type of output messages. +- `~target_append_suffix` (bool, default false): If true, the output topic will be suffixed with a metadata-based + string. +- `~target_frame` (str, default: no change): TF frame to transform the messages to. Please note that frames that are + too "titled" from gravity will not make much sense. +- `~subscribe_fix` (bool, default true): Whether to subscribe `fix` topic. In some cases, you don't need it. +- `~subscribe_utm` (bool, default true): Whether to subscribe `utm_zone` topic. It is fully optional. +- `~input_orientation` (str, 'enu' or 'ned', default: unspecified): ENU or NED orientation to be used to interpret + input messages (in case orientation cannot be + derived either from message contents or topic + name). +- `~input_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used to + interpret input messages (in case + reference cannot be derived either + from message contents or topic + name). +- `~input_variance` (double, optional, rad^2): If specified, this variance will be used in the output messages + if variance cannot be determined from the input messages (e.g. for + `QuaternionStamped`). +- `~strict` (bool, default true): If true, conversions between magnetic and geographic North will fail if the used + magnetic model is used outside its declared bounds of validity (mostly year and + altitude). +- All parameters consumed by `CompassConverter` (most interesting are `initial_lat`, `initial_lon`, that can relieve + this nodelet from subscribing `fix` topic, if you know the approximate coordinates in advance). \ No newline at end of file diff --git a/compass_conversions/include/compass_conversions/compass_converter.h b/compass_conversions/include/compass_conversions/compass_converter.h new file mode 100644 index 0000000..72f16e0 --- /dev/null +++ b/compass_conversions/include/compass_conversions/compass_converter.h @@ -0,0 +1,356 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Convert between various compass representations. + * \author Martin Pecka + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace compass_conversions +{ + +struct CompassConverterPrivate; + +/** + * \brief 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(). + */ +class CompassConverter : public cras::HasLogger +{ +public: + /** + * \brief Create the compass converter. + * + * \param[in] log The logger. + * \param[in] strict Whether to fail if the magnetic model is used outside its natural validity bounds. + */ + CompassConverter(const cras::LogHelperPtr& log, bool strict); + virtual ~CompassConverter(); + + /** + * \brief Configure the compass converter from the given ROS parameters. + * + * \param[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. + */ + void configFromParams(const cras::BoundParamHelper& params); + + /** + * \brief Force magnetic declination instead of computing it. + * \param[in] declination The forced declination in rad. If nullopt, declination is set to be computed. + */ + virtual void forceMagneticDeclination(const cras::optional& declination); + + /** + * \brief Force UTM grid convergence instead of computing it. + * \param[in] convergence The forced convergence in rad. If nullopt, convergence is set to be computed. + */ + virtual void forceUTMGridConvergence(const cras::optional& convergence); + + /** + * \brief Set the path where magnetic models are stored. + * \param[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 void setMagneticModelPath(const cras::optional& modelPath); + + /** + * \brief Force using the given magnetic model instead of automatically selecting the best one. + * \param[in] model The name of the model to force (e.g. "wmm2020"). If empty, the best model will be autoselected. + */ + virtual void forceMagneticModelName(const std::string& model); + + /** + * \brief Set whether UTM zone should be kept after it is first computed. + * \param[in] keep If true, the next call to setNavSatPos() will store the computed UTM zone to forcedUTMZone. + */ + virtual void setKeepUTMZone(bool keep); + + /** + * \brief Callback for GNSS fix (so that the converter can compute UTM grid convergence and store the pose). + * \param[in] fix The fix message. Only `latitude`, `longitude`, `altitude` and `header.stamp` are relevant. + * \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. + */ + virtual void setNavSatPos(const sensor_msgs::NavSatFix& fix); + + /** + * \brief Get the value of magnetic declination for the last location received by setNatSatPos(). + * \param[in] stamp The time for which declination is queried. + * \return The magnetic declination in radians or an error message. + * \note If forcedMagneticDeclination is set, it is returned without any adjustments. + */ + virtual cras::expected getMagneticDeclination(const ros::Time& stamp) const; + + /** + * \brief Compute magnetic declination for the given position and time. + * \param[in] fix The position to compute declination for. + * \param[in] stamp The time to compute declination for. + * \return The magnetic declination in radians or an error message. + * \note This function does not take forcedMagneticDeclination into account. + */ + virtual cras::expected computeMagneticDeclination( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const; + + /** + * \brief Get the value of UTM grid convergence for the last location received by setNatSatPos(). + * \return The UTM grid convergence in radians, or an error message. + * \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. + */ + virtual cras::expected getUTMGridConvergence() const; + + /** + * \brief Get the UTM zone of the last location received by setNatSatPos(). + * \return The UTM zone, or an error message. + * \note If forcedUTMZone is set, it will be directly returned. + */ + virtual cras::expected getUTMZone() const; + + /** + * \brief Set the UTM zone that will be used for all future UTM operations. + * \param[in] zone The UTM zone. If nullopt, UTM operations will use the default zone. + * \note This function sets forcedUTMZone. + */ + virtual void forceUTMZone(const cras::optional& zone); + + /** + * \brief Get the value of UTM grid convergence and UTM zone for the provided place. + * \param[in] fix The place for which grid convergence is queried. + * \param[in] utmZone Optional forced UTM zone. If not specified, the default UTM zone will be used. + * \return The UTM grid convergence in radians and corresponding UTM zone, or an error message. + * \note This function does not take forcedUTMGridConvergence into account. + */ + virtual cras::expected, std::string> computeUTMGridConvergenceAndZone( + const sensor_msgs::NavSatFix& fix, const cras::optional& utmZone) const; + + /** + * \brief Convert the given compass_msgs::Azimuth message parametrized by the given unit, orientation and reference. + * + * \param[in] azimuth The input azimuth message. + * \param[in] unit The output azimuth units. + * \param[in] orientation The output azimuth orientation. + * \param[in] reference The output azimuth reference. + * \return The converted Azimuth message or an error message. + * + * \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. + */ + virtual cras::expected 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; + + /** + * \brief Convert the given geometry_msgs::QuaternionStamped message to Azimuth parametrized by the given unit, + * orientation and reference. + * + * \param[in] quat The input quaternion message. + * \param[in] variance Variance of the measurement (in rad^2). + * \param[in] unit The output azimuth units. + * \param[in] orientation The output azimuth orientation (this is just a declaration, no conversion is done). + * \param[in] reference The output azimuth reference (this is just a declaration, no conversion is done). + * \return The converted Azimuth message or an error message. + */ + virtual cras::expected 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; + + /** + * \brief Convert the given geometry_msgs::Quaternion message to Azimuth parametrized by the given unit, + * orientation and reference. + * + * \param[in] quat The input quaternion message. + * \param[in] header Header of the output message. + * \param[in] variance Variance of the measurement (in rad^2). + * \param[in] unit The output azimuth units. + * \param[in] orientation The output azimuth orientation (this is just a declaration, no conversion is done). + * \param[in] reference The output azimuth reference (this is just a declaration, no conversion is done). + * \return The converted Azimuth message or an error message. + */ + virtual cras::expected 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; + + /** + * \brief Convert a received geometry_msgs::QuaternionStamped message to Azimuth. If needed, extract the orientation + * and reference from the topic name. + * + * \param[in] quatEvent The input quaternion message. + * \param[in] variance Variance of the measurement (in rad^2). + * \param[in] unit The output azimuth units. + * \param[in] orientation The declared input orientation (autodetected from topic if not specified). + * \param[in] reference The declared input reference (autodetected from topic if not specified). + * \return The converted Azimuth message or an error message. + */ + cras::expected convertQuaternionMsgEvent( + const ros::MessageEvent& quatEvent, + decltype(compass_msgs::Azimuth::variance) variance = 0, + decltype(compass_msgs::Azimuth::unit) unit = compass_msgs::Azimuth::UNIT_RAD, + const cras::optional& orientation = {}, + const cras::optional& reference = {}) const; + + /** + * \brief Convert a received geometry_msgs::PoseWithCovarianceStamped message to Azimuth. If needed, extract the + * orientation and reference from the topic name. + * + * \param[in] poseEvent The input pose message. + * \param[in] unit The output azimuth units. + * \param[in] orientation The declared input orientation (autodetected from topic if not specified). + * \param[in] reference The declared input reference (autodetected from topic if not specified). + * \return The converted Azimuth message or an error message. + */ + cras::expected convertPoseMsgEvent( + const ros::MessageEvent& poseEvent, + decltype(compass_msgs::Azimuth::unit) unit = compass_msgs::Azimuth::UNIT_RAD, + const cras::optional& orientation = {}, + const cras::optional& reference = {}) const; + + /** + * \brief Convert a received sensor_msgs::Imu message to Azimuth. If needed, extract the orientation and reference + * from the topic name. + * + * \param[in] imuEvent The input IMU message. + * \param[in] unit The output azimuth units. + * \param[in] orientation The declared input orientation (autodetected from topic if not specified). + * \param[in] reference The declared input reference (autodetected from topic if not specified). + * \return The converted Azimuth message or an error message. + */ + cras::expected convertImuMsgEvent( + const ros::MessageEvent& imuEvent, + decltype(compass_msgs::Azimuth::unit) unit = compass_msgs::Azimuth::UNIT_RAD, + const cras::optional& orientation = {}, + const cras::optional& reference = {}) const; + + /** + * \brief 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. + * + * \param[in] event The input message. + * \param[in] variance Variance of the measurement (in rad^2) (if it isn't a part of the message). + * \param[in] unit The output azimuth units. + * \param[in] orientation The declared input orientation (autodetected from topic if not specified). + * \param[in] reference The declared input reference (autodetected from topic if not specified). + * \return The converted Azimuth message or an error message. + */ + cras::expected convertUniversalMsgEvent( + const ros::MessageEvent& event, + decltype(compass_msgs::Azimuth::variance) variance = 0, + decltype(compass_msgs::Azimuth::unit) unit = compass_msgs::Azimuth::UNIT_RAD, + const cras::optional& orientation = {}, + const cras::optional& reference = {}) const; + + /** + * \brief Convert the given Azimuth message to geometry_msgs::QuaternionStamped in the same parametrization. + * + * \param[in] azimuth The input azimuth message. + * \return The converted geometry_msgs::QuaternionStamped message or an error message. + */ + virtual cras::expected convertToQuaternion( + const compass_msgs::Azimuth& azimuth) const; + + /** + * \brief Convert the given Azimuth message to geometry_msgs::PoseWithCovarianceStamped in the same parametrization. + * + * \param[in] azimuth The input azimuth message. + * \return The converted geometry_msgs::PoseWithCovarianceStamped message or an error message. + */ + virtual cras::expected convertToPose( + const compass_msgs::Azimuth& azimuth) const; + + /** + * \brief Convert the given Azimuth message to sensor_msgs::Imu in the same parametrization. + * + * \param[in] azimuth The input azimuth message. + * \return The converted sensor_msgs::Imu message or an error message. + */ + virtual cras::expected convertToImu(const compass_msgs::Azimuth& azimuth) const; + +protected: + //! \brief UTM convergence of the last received navsat position (or the forced one). + cras::optional lastUTMGridConvergence; + + //! \brief Last determined UTM zone. If empty, no zone has been determined yet. + cras::optional lastUTMZone; + + //! \brief The user-forced magnetic declination (if set, do not compute it). + cras::optional forcedMagneticDeclination; + + //! \brief The user-forced UTM grid convergence (if set, do not compute it). + cras::optional forcedUTMGridConvergence; + + //! \brief The user-forced UTM zone (if set, do not compute it). + cras::optional forcedUTMZone; + + //! \brief If true, the first determined UTM zone will be kept for all future queries. + bool keepUTMZone {true}; + + //! \brief If the user forces a magnetic model, this is its name. + std::string forcedMagneticModelName{}; + + //! \brief If true, convertAzimuth() will fail when the magnetic model is used outside its bounds. + bool strict {true}; + + //! \brief Last received GNSS fix. Used for determining magnetic declination and UTM grid convergence. + cras::optional lastFix; + + //! \brief PIMPL data + std::unique_ptr data; +}; + +} diff --git a/compass_conversions/include/compass_conversions/message_filter.h b/compass_conversions/include/compass_conversions/message_filter.h new file mode 100644 index 0000000..c867110 --- /dev/null +++ b/compass_conversions/include/compass_conversions/message_filter.h @@ -0,0 +1,309 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Message filter to convert between various compass representations. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace compass_conversions +{ + +/** + * \brief message_filters subscriber that can subscribe to various topic types and convert them all to an Azimuth + * message. + * + * Currently supported types are: compass_msgs::Azimuth, geometry_msgs::PoseWithCovarianceStamped, + * geometry_msgs::QuaternionStamped, sensor_msgs::Imu. + */ +class UniversalAzimuthSubscriber : + public message_filters::SimpleFilter, public message_filters::SubscriberBase, + public cras::HasLogger +{ +public: + typedef ros::MessageEvent EventType; + + /** + * \brief Constructor + * + * \param nh The ros::NodeHandle to use for subscribing. + * \param topic The topic to subscribe to. + * \param queueSize Queue size of the subscription. + * \param transportHints The transport hints to pass to the subscriber. + * \param callbackQueue The callback queue to attach to. + */ + UniversalAzimuthSubscriber(const cras::LogHelperPtr& log, ros::NodeHandle& nh, const std::string& topic, + uint32_t queueSize, const ros::TransportHints& transportHints = {}, + ros::CallbackQueueInterface* callbackQueue = nullptr); + + ~UniversalAzimuthSubscriber() override; + + /** + * \brief Subscribe to a topic. + * + * If this Subscriber is already subscribed to a topic, this function will first unsubscribe. + * + * \param nh The ros::NodeHandle to use for subscribing. + * \param topic The topic to subscribe to. + * \param queueSize Queue size of the subscription. + * \param transportHints The transport hints to pass to the subscriber. + * \param callbackQueue The callback queue to attach to. + */ + void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queueSize, + const ros::TransportHints& transportHints, ros::CallbackQueueInterface* callbackQueue) override; + + /** + * \brief Re-subscribe to a topic. + */ + void subscribe() override; + + /** + * \brief Unsubscribe from the topic. + */ + void unsubscribe() override; + + /** + * \brief Set defaults for inputs which do not support autodetection of various azimuth properties. + * + * \param[in] orientation The default orientation used if it cannot be detected. + * \param[in] reference The reference used if it cannot be detected. + * \param[in] variance Default variance used for topics which cannot automatically discover it. + */ + void setInputDefaults( + const cras::optional& orientation, + const cras::optional& reference, + const cras::optional& variance); + + /** + * \brief Configure the subscriber from ROS parameters. + * \param[in] params ROS parameters. + * + * Supported parameters: + * - `~input_orientation` (str, 'enu' or 'ned', default: unspecified): ENU or NED orientation to be used to interpret + * input messages (in case orientation cannot be + * derived either from message contents or topic + * name). + * - `~input_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used to + * interpret input messages (in + * case reference cannot be derived + * either from message contents or + * topic name). + * - `~input_variance` (double, optional, rad^2): If specified, this variance will be used in the output messages + * if variance cannot be determined from the input messages (e.g. for + * `QuaternionStamped`). + */ + void configFromParams(const cras::BoundParamHelper& params); + + /** + * \brief Get the name of the subscribed topic. + * \return The topic name. + */ + std::string getTopic() const; + + /** + * \brief Returns the internal ros::Subscriber. + */ + const ros::Subscriber& getSubscriber() const; + + template + void connectInput(F& f) + { + } + + void add(const EventType& event); + +protected: + void cb(const EventType& event); + + ros::Subscriber sub; //!< The ROS subscriber. + ros::SubscribeOptions subscribeOps; //!< Options for recreating the subscriber. + ros::NodeHandle nh; //!< The nodehandle to use for subscribing, + CompassConverter converter; //!< The azimuth message converter. + + //! Orientation of the input azimuth (in case it is a data type which does not tell orientation explicitly). + cras::optional inputOrientation; + + //! Reference of the input azimuth (in case it is a data type which does not tell reference explicitly). + cras::optional inputReference; + + //! Variance of the input azimuth (in case it is a data type which does not tell reference explicitly). + cras::optional inputVariance; +}; + +/** + * \brief Message filter to convert between various compass representations. + * + * \sa https://wiki.ros.org/message_filters + * + * Example usage: + * \code{.cpp} + * message_filters::UniversalAzimuthSubscriber azimuthInput(...); + * message_filters::Subscriber fixInput(...); + * auto converter = std::make_shared(log, true); + * // converter->configFromParams(params); + * compass_conversions::CompassFilter filter(log, converter, azimuthInput, fixInput, + * compass_msgs::Azimuth::UNIT_RAD, compass_msgs::Azimuth::ORIENTATION_ENU, + * compass_msgs::Azimuth::REFERENCE_GEOGRAPHIC); + * filter.registerCallback([](const compass_msgs::AzimuthConstPtr& msg) { + * ... // Handle the data + * }); + * \endcode + */ +class CompassFilter : public message_filters::SimpleFilter, public cras::HasLogger +{ +public: + typedef ros::MessageEvent AzimuthEventType; + typedef ros::MessageEvent FixEventType; + typedef ros::MessageEvent UTMZoneEventType; + + /** + * \brief Construct azimuth filter that can convert all parameters. + * + * \tparam AzimuthInput The type of the input filter. + * \tparam FixInput The type of the navsat fix filter. + * \tparam UTMZoneInput The type of the UTM Zone filter. + * \param[in] log Logger. + * \param[in] converter The azimuth converter instance. If nullptr, a default converter is constructed. + * \param[in] azimuthInput The message filter producing azimuth messages. + * \param[in] fixInput The message filter producing fix messages. + * \param[in] utmZoneInput The message filter producing UTM zone messages. + * \param[in] unit The output azimuth unit. + * \param[in] orientation The output azimuth orientation. + * \param[in] reference The output azimuth reference. + */ + template + CompassFilter(const cras::LogHelperPtr& log, const std::shared_ptr& converter, + AzimuthInput& azimuthInput, FixInput& fixInput, UTMZoneInput& utmZoneInput, + decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, + decltype(compass_msgs::Azimuth::reference) reference): + HasLogger(log), converter(converter), unit(unit), orientation(orientation), reference(reference) + { + if (this->converter == nullptr) + this->converter = std::make_shared(log, true); + this->connectAzimuthInput(azimuthInput); + this->connectFixInput(fixInput); + this->connectUTMZoneInput(utmZoneInput); + } + + /** + * \brief Construct azimuth filter that can convert all parameters. + * + * \tparam AzimuthInput The type of the input filter. + * \tparam FixInput The type of the navsat fix filter. + * \param[in] log Logger. + * \param[in] converter The azimuth converter instance. If nullptr, a default converter is constructed. + * \param[in] azimuthInput The message filter producing azimuth messages. + * \param[in] fixInput The message filter producing fix messages. + * \param[in] unit The output azimuth unit. + * \param[in] orientation The output azimuth orientation. + * \param[in] reference The output azimuth reference. + */ + template + CompassFilter(const cras::LogHelperPtr& log, const std::shared_ptr& converter, + AzimuthInput& azimuthInput, FixInput& fixInput, + decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, + decltype(compass_msgs::Azimuth::reference) reference): + HasLogger(log), converter(converter), unit(unit), orientation(orientation), reference(reference) + { + if (this->converter == nullptr) + this->converter = std::make_shared(log, true); + this->connectAzimuthInput(azimuthInput); + this->connectFixInput(fixInput); + } + + /** + * \brief Construct azimuth filter that can only convert units and orientation. + * + * \tparam AzimuthInput The type of the input filter. + * \param[in] log Logger. + * \param[in] converter The azimuth converter instance. If nullptr, a default converter is constructed. + * \param[in] azimuthInput The message filter producing azimuth messages. + * \param[in] unit The output azimuth unit. + * \param[in] orientation The output azimuth orientation. + */ + template + CompassFilter(const cras::LogHelperPtr& log, const std::shared_ptr& converter, + AzimuthInput& azimuthInput, + decltype(compass_msgs::Azimuth::unit) unit, decltype(compass_msgs::Azimuth::orientation) orientation, + decltype(compass_msgs::Azimuth::reference) reference): + HasLogger(log), converter(converter), unit(unit), orientation(orientation), reference(reference) + { + if (this->converter == nullptr) + this->converter = std::make_shared(log, true); + this->connectAzimuthInput(azimuthInput); + } + + virtual ~CompassFilter(); + + template + void connectAzimuthInput(AzimuthInput& f) + { + this->azimuthConnection.disconnect(); + // The explicit cast to boost:function is needed to retain the message event metadata + this->azimuthConnection = f.registerCallback( + boost::function(cras::bind_front(&CompassFilter::cbAzimuth, this))); + } + + template + void connectFixInput(FixInput& f) + { + this->fixConnection.disconnect(); + // The explicit cast to boost:function is needed to retain the message event metadata + this->fixConnection = f.registerCallback( + boost::function(cras::bind_front(&CompassFilter::cbFix, this))); + } + + template + void connectUTMZoneInput(UTMZoneInput& f) + { + this->utmZoneConnection.disconnect(); + // The explicit cast to boost:function is needed to retain the message event metadata + this->utmZoneConnection = f.registerCallback( + boost::function(cras::bind_front(&CompassFilter::cbUTMZone, this))); + } + +protected: + virtual void cbAzimuth(const AzimuthEventType& azimuthEvent); + virtual void cbFix(const FixEventType& fixEvent); + virtual void cbUTMZone(const UTMZoneEventType& utmZoneEvent); + + message_filters::Connection azimuthConnection; //!< Connection to the azimuth input. + message_filters::Connection fixConnection; //!< Connection to the navsat fix input. + message_filters::Connection utmZoneConnection; //!< Connection to the UTM zone input. + + std::shared_ptr converter; //!< The compass converter instance. + bool fixReceived {false}; //!< Whether at least one navsat fix message has been received. + + decltype(compass_msgs::Azimuth::unit) unit; //!< The target azimuth unit. + decltype(compass_msgs::Azimuth::orientation) orientation; //!< The target azimuth orientation. + + //! The target azimuth reference (unchanged if empty). + cras::optional reference; +}; + +} diff --git a/compass_conversions/include/compass_conversions/tf2_compass_msgs.h b/compass_conversions/include/compass_conversions/tf2_compass_msgs.h new file mode 100644 index 0000000..df0b889 --- /dev/null +++ b/compass_conversions/include/compass_conversions/tf2_compass_msgs.h @@ -0,0 +1,30 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Support for transforming compass_msgs::Azimuth messages. + * \author Martin Pecka + */ + +#include + +#include +#include +#include +#include + +namespace tf2 +{ +template<> const ros::Time& getTimestamp(const compass_msgs::Azimuth& t); +template<> const std::string& getFrameId(const compass_msgs::Azimuth& t); + +compass_msgs::Azimuth toMsg(const compass_msgs::Azimuth& in); +void fromMsg(const compass_msgs::Azimuth& msg, compass_msgs::Azimuth& out); + +template<> +void doTransform( + const compass_msgs::Azimuth& t_in, compass_msgs::Azimuth& t_out, const geometry_msgs::TransformStamped& transform); +} diff --git a/compass_conversions/include/compass_conversions/topic_names.h b/compass_conversions/include/compass_conversions/topic_names.h new file mode 100644 index 0000000..4f8c065 --- /dev/null +++ b/compass_conversions/include/compass_conversions/topic_names.h @@ -0,0 +1,72 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Logic for naming topics according to the type of Azimuth message they carry. + * \author Martin Pecka + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace compass_conversions +{ + +/** + * \brief Get the suffix of topic name that identifies the particular representation of azimuth. + * + * \tparam T Type of the message carrying the azimuth information. + * \param[in] unit Angular units (only make sense for Azimuth messages). + * \param[in] orientation ENU or NED orientation of the world. + * \param[in] reference What North reference does the azimuth use. + * \return The suffix. + */ +template::value || + std::is_same::value || + std::is_same::value || + std::is_same::value + >* = nullptr> +std::string getAzimuthTopicSuffix( + decltype(compass_msgs::Azimuth::unit) unit, + decltype(compass_msgs::Azimuth::orientation) orientation, + decltype(compass_msgs::Azimuth::reference) reference); + +/** + * \brief Autodetect azimuth representation from the name of the topic on which the message came. + * + * \param[in] topic The topic to parse. + * \return The autodetected parameters, or nullopt if autodetection failed. + */ +cras::optional> parseAzimuthTopicName(const std::string& topic); + +/** + * \brief Autodetect azimuth representation from connection header of a topic it came on. + * + * \param[in] connectionHeaderPtr Pointer to the connection header, should contain key "topic". + * \return The autodetected parameters, or nullopt if autodetection failed. + */ +cras::optional> parseAzimuthTopicName(const boost::shared_ptr& connectionHeaderPtr); + +} diff --git a/compass_conversions/nodelets.xml b/compass_conversions/nodelets.xml new file mode 100644 index 0000000..f2da3fb --- /dev/null +++ b/compass_conversions/nodelets.xml @@ -0,0 +1,10 @@ + + + + + + + Nodelet transforming Azimuth message to different TF frames or representations. + + diff --git a/compass_conversions/nodelets/compass_transformer.cpp b/compass_conversions/nodelets/compass_transformer.cpp new file mode 100644 index 0000000..ad40e2e --- /dev/null +++ b/compass_conversions/nodelets/compass_transformer.cpp @@ -0,0 +1,309 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Support for transforming compass_msgs::Azimuth messages. + * \author Martin Pecka + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; +using Fix = sensor_msgs::NavSatFix; + +namespace compass_conversions +{ + +enum class OutputType +{ + Azimuth, + Imu, + Pose, + Quaternion, +}; + +OutputType parseOutputType(const std::string& outputType) +{ + const auto output = cras::toLower(outputType); + if (output == "azimuth") + return OutputType::Azimuth; + else if (output == "imu") + return OutputType::Imu; + else if (output == "pose") + return OutputType::Pose; + else if (output == "quaternion" || output == "quat") + return OutputType::Quaternion; + else + throw std::runtime_error("Unknown output type: " + outputType); +} + +std::string outputTypeToString(const OutputType type) +{ + switch (type) + { + case OutputType::Azimuth: + return "azimuth"; + case OutputType::Imu: + return "imu"; + case OutputType::Pose: + return "pose"; + case OutputType::Quaternion: + return "quaternion"; + default: + throw std::runtime_error(cras::format("Unknown output type: %d", static_cast(type))); + } +} + +/** + * \brief Nodelet for transforming one type and parametrization of azimuth to another type, parametrization and + * TF frame. + * + * Subscribed topics: + * - `~azimuth_in` (compass_msgs/Azimuth or geometry_msgs/QuaternionStamped or geometry_msgs/PoseWithCovarianceStamped + * or sensor_msgs/Imu): The input azimuth. The name of the topic (if you remap it) can be used to autodetect some + * metadata for the conversion. + * - `fix` (sensor_msgs/NavSatFix): GNSS fix messages that can be used to determine some parameters for the conversion. + * - `utm_zone` (std_msgs/Int32): Optional messages with forced UTM zone. + * - TF (only if `~target_frame` is nonempty) + * + * Published topics: + * - `~azimuth_out` or `~azimuth_out/SUFFIX`: The transformed azimuth. If `~target_append_suffix` is true, the variant + * with topic name suffix will be used (e.g. `~azimuth_out/mag/enu/deg`). + * The type of the published message is determined by `~target_type`. + * + * Parameters: + * - `~queue_size` (int, default 10): Queue size for the subscribers and publishers. + * - `~target_unit` (str, 'deg' or 'rad', default: 'rad'): Angular unit to be used in the transformed messages. + * - `~target_orientation` (str, 'enu' or 'ned', default: 'enu'): ENU or NED orientation to be used in the + * transformed messages. + * - `~target_reference` (str, 'magnetic', 'geographic' or 'UTM', default: 'geographic'): North reference to be used in + * the transformed messages. + * - `~target_type` (str, 'azimuth', 'quaternion', 'pose' or 'imu', default 'azimuth'): The Type of output messages. + * - `~target_append_suffix` (bool, default false): If true, the output topic will be suffixed with a metadata-based + * string. + * - `~target_frame` (str, default: no change): TF frame to transform the messages to. Please note that frames that are + * too "titled" from gravity will not make much sense. + * - `~subscribe_fix` (bool, default true): Whether to subscribe `fix` topic. In some cases, you don't need it. + * - `~subscribe_utm` (bool, default true): Whether to subscribe `utm_zone` topic. It is fully optional. + * - `~input_orientation` (str, 'enu' or 'ned', default: unspecified): ENU or NED orientation to be used to interpret + * input messages (in case orientation cannot be + * derived either from message contents or topic + * name). + * - `~input_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used to + * interpret input messages (in case + * reference cannot be derived either + * from message contents or topic + * name). + * - `~input_variance` (double, optional, rad^2): If specified, this variance will be used in the output messages + * if variance cannot be determined from the input messages (e.g. for + * `QuaternionStamped`). + * - `~strict` (bool, default true): If true, conversions between magnetic and geographic North will fail if the used + * magnetic model is used outside its declared bounds of validity (mostly year and + * altitude). + * - All parameters consumed by `CompassConverter` (most interesting are `initial_lat`, `initial_lon`, that can relieve + * this nodelet from subscribing `fix` topic, if you know the approximate coordinates in advance). + */ +class CompassTransformerNodelet : public cras::Nodelet +{ +protected: + void onInit() override + { + cras::Nodelet::onInit(); + + const auto params = this->privateParams(); + + // Start reading params + + const auto queue_size = params->getParam("queue_size", 10_sz, "messages"); + auto nh = this->getNodeHandle(); + auto pnh = this->getPrivateNodeHandle(); + + const auto targetUnit = params->getParam("target_unit", Az::UNIT_RAD, "", + {.resultToStr = &compass_msgs::unitToString, .toResult = &compass_msgs::parseUnit}); + + const auto targetOrientation = params->getParam( + "target_orientation", Az::ORIENTATION_ENU, "", + {.resultToStr = &compass_msgs::orientationToString, .toResult = &compass_msgs::parseOrientation}); + + const auto targetReference = params->getParam( + "target_reference", Az::REFERENCE_GEOGRAPHIC, "", + {.resultToStr = &compass_msgs::referenceToString, .toResult = &compass_msgs::parseReference}); + + this->targetType = params->getParam("target_type", this->targetType, "", + {.resultToStr = &outputTypeToString, .toResult = &parseOutputType}); + + const auto targetAppendSuffix = params->getParam("target_append_suffix", false); + + this->targetFrame = params->getParam("target_frame", std::string()); + + const auto subscribeFix = params->getParam("subscribe_fix", true); + const auto subscribeUTMZone = params->getParam("subscribe_utm", true); + + // End reading params + + const auto log = this->getLogger(); + + this->converter = std::make_shared(log, params->getParam("strict", true)); + this->converter->configFromParams(*params); + + auto outputNh = targetAppendSuffix ? ros::NodeHandle(pnh, "azimuth_out") : pnh; + + std::string outputTopicSuffix; + switch (this->targetType) + { + case OutputType::Imu: + outputTopicSuffix = getAzimuthTopicSuffix(targetUnit, targetOrientation, targetReference); + this->pub = outputNh.advertise( + targetAppendSuffix ? outputTopicSuffix : "azimuth_out", queue_size); + break; + case OutputType::Pose: + outputTopicSuffix = getAzimuthTopicSuffix( + targetUnit, targetOrientation, targetReference); + this->pub = outputNh.advertise( + targetAppendSuffix ? outputTopicSuffix : "azimuth_out", queue_size); + break; + case OutputType::Quaternion: + outputTopicSuffix = getAzimuthTopicSuffix( + targetUnit, targetOrientation, targetReference); + this->pub = outputNh.advertise( + targetAppendSuffix ? outputTopicSuffix : "azimuth_out", queue_size); + break; + default: + outputTopicSuffix = getAzimuthTopicSuffix(targetUnit, targetOrientation, targetReference); + this->pub = outputNh.advertise( + targetAppendSuffix ? outputTopicSuffix : "azimuth_out", queue_size); + break; + } + + this->azimuthInput = std::make_unique(this->log, pnh, "azimuth_in", queue_size); + this->azimuthInput->configFromParams(*params); + + this->compassFilter = std::make_unique( + log, this->converter, *this->azimuthInput, targetUnit, targetOrientation, targetReference); + + if (subscribeFix) + { + this->fixInput = std::make_unique>(nh, "fix", queue_size); + this->compassFilter->connectFixInput(*this->fixInput); + } + + if (subscribeUTMZone) + { + this->utmZoneInput = std::make_unique>(nh, "utm_zone", queue_size); + this->compassFilter->connectUTMZoneInput(*this->utmZoneInput); + } + + if (targetFrame.empty()) + { + this->compassFilter->registerCallback(&CompassTransformerNodelet::publish, this); + } + else + { + this->tfFilter = std::make_unique>( + log, *this->compassFilter, this->getBuffer().getRawBuffer(), targetFrame, queue_size, nh); + this->tfFilter->registerCallback(&CompassTransformerNodelet::transformAndPublish, this); + this->tfFilter->registerFailureCallback(cras::bind_front(&CompassTransformerNodelet::failedCb, this)); + } + + CRAS_INFO("Publishing azimuth to topic %s (type %s).", + ros::names::resolve(this->pub.getTopic()).c_str(), outputTypeToString(this->targetType).c_str()); + } + + void publish(const Az::ConstPtr& msg) + { + switch (this->targetType) + { + case OutputType::Imu: + { + const auto maybeImu = this->converter->convertToImu(*msg); + if (maybeImu.has_value()) + this->pub.publish(*maybeImu); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybeImu.error().c_str()); + break; + } + case OutputType::Pose: + { + const auto maybePose = this->converter->convertToPose(*msg); + if (maybePose.has_value()) + this->pub.publish(*maybePose); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybePose.error().c_str()); + break; + } + case OutputType::Quaternion: + { + const auto maybeQuat = this->converter->convertToQuaternion(*msg); + if (maybeQuat.has_value()) + this->pub.publish(*maybeQuat); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybeQuat.error().c_str()); + break; + } + default: + this->pub.publish(msg); + break; + } + } + + void transformAndPublish(const Az::ConstPtr& msg) + { + try + { + Az::Ptr outMsg(new Az{}); + *outMsg = this->getBuffer().transform(*msg, this->targetFrame, ros::Duration(0.1)); + this->publish(outMsg); + } + catch (const tf2::TransformException& e) + { + CRAS_WARN_THROTTLE(1.0, "Azimuth transformation failed: %s", e.what()); + } + } + + void failedCb(const Az::ConstPtr& /*msg*/, const tf2_ros::filter_failure_reasons::FilterFailureReason reason) + { + CRAS_WARN_THROTTLE(1.0, "Can't transform incoming Azimuth data to frame %s. Reason %d", + this->targetFrame.c_str(), reason); + } + + std::shared_ptr converter; + std::unique_ptr azimuthInput; + std::unique_ptr> fixInput; + std::unique_ptr> utmZoneInput; + std::unique_ptr compassFilter; + std::unique_ptr> tfFilter; + ros::Publisher pub; + std::string targetFrame; + OutputType targetType {OutputType::Azimuth}; +}; + +} + +PLUGINLIB_EXPORT_CLASS(compass_conversions::CompassTransformerNodelet, nodelet::Nodelet) diff --git a/compass_conversions/package.xml b/compass_conversions/package.xml new file mode 100644 index 0000000..ee6ebcb --- /dev/null +++ b/compass_conversions/package.xml @@ -0,0 +1,55 @@ + + + + + compass_conversions + 1.0.3 + Common conversions for compass data. + + Martin Pecka + Martin Pecka + + BSD + + https://github.com/ctu-vras/compass + https://github.com/ctu-vras/compass/issues + https://wiki.ros.org/compass_conversions + + catkin + + angles + geographiclib + magnetic_model + nodelet + pluginlib + tf2_ros + tf2_geometry_msgs + + angles + geographiclib + magnetic_model + nodelet + pluginlib + tf2_ros + tf2_geometry_msgs + + compass_msgs + cras_cpp_common + geometry_msgs + message_filters + roscpp + sensor_msgs + std_msgs + tf2 + topic_tools + + python-catkin-lint + python3-catkin-lint + roslint + rostest + + + + + + diff --git a/compass_conversions/rosdoc.yaml b/compass_conversions/rosdoc.yaml new file mode 100644 index 0000000..8211934 --- /dev/null +++ b/compass_conversions/rosdoc.yaml @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +- builder: doxygen + required: True + file_patterns: '*.h *.hpp *.hh *.dox /README.md' + use_mdfile_as_mainpage: 'README.md' + generate_treeview: True diff --git a/compass_conversions/src/compass_converter.cpp b/compass_conversions/src/compass_converter.cpp new file mode 100644 index 0000000..a4dba37 --- /dev/null +++ b/compass_conversions/src/compass_converter.cpp @@ -0,0 +1,577 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Convert between various compass representations. + * \author Martin Pecka + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace compass_conversions +{ + +using Az = compass_msgs::Azimuth; +using Imu = sensor_msgs::Imu; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Quat = geometry_msgs::QuaternionStamped; + +/** + * \brief Private data of CompassConverter. + */ +struct CompassConverterPrivate +{ + //! \brief Magnetic model manager. + std::unique_ptr magneticModelManager; + + //! \brief Cache of already initialized magnetic field models. Keys are years. + std::map> magneticModels; +}; + +CompassConverter::CompassConverter(const cras::LogHelperPtr& log, const bool strict) : + cras::HasLogger(log), strict(strict), data(new CompassConverterPrivate{}) +{ + this->data->magneticModelManager = std::make_unique(this->log); +} + +CompassConverter::~CompassConverter() = default; + +void CompassConverter::configFromParams(const cras::BoundParamHelper& params) +{ + cras::TempLocale l(LC_ALL, "en_US.UTF-8"); // Support printing ° signs + + if (params.hasParam("magnetic_declination")) + this->forceMagneticDeclination(params.getParam("magnetic_declination", cras::nullopt, "rad")); + else + this->forcedMagneticModelName = params.getParam("magnetic_model", std::string()); + + if (params.hasParam("magnetic_models_path")) + this->setMagneticModelPath(params.getParam("magnetic_models_path", cras::nullopt)); + + if (params.hasParam("utm_grid_convergence")) + this->forceUTMGridConvergence(params.getParam("utm_grid_convergence", cras::nullopt, "rad")); + + if (params.hasParam("utm_zone")) + this->forceUTMZone(params.getParam("utm_zone", cras::nullopt)); + + this->setKeepUTMZone(params.getParam("keep_utm_zone", this->keepUTMZone)); + + if (!this->forcedMagneticDeclination.has_value() || !this->forcedUTMGridConvergence.has_value()) + { + if (params.hasParam("initial_lat") && params.hasParam("initial_lon")) + { + sensor_msgs::NavSatFix msg; + + msg.latitude = params.getParam("initial_lat", cras::nullopt, "°"); + msg.longitude = params.getParam("initial_lon", cras::nullopt, "°"); + msg.altitude = params.getParam("initial_alt", 0.0, "m"); + + std::list computedValues; + if (!this->forcedMagneticDeclination.has_value()) + computedValues.emplace_back("magnetic declination"); + if (!this->forcedUTMGridConvergence.has_value()) + computedValues.emplace_back("UTM grid convergence"); + + CRAS_INFO( + "Initial GPS coords for computation of %s are %.6f°, %.6f°, altitude %.0f m.", + cras::join(computedValues, "and").c_str(), msg.latitude, msg.longitude, msg.altitude); + + this->setNavSatPos(msg); + } + } +} + +void CompassConverter::forceMagneticDeclination(const cras::optional& declination) +{ + this->forcedMagneticDeclination = declination; +} + +void CompassConverter::forceUTMGridConvergence(const cras::optional& convergence) +{ + this->forcedUTMGridConvergence = this->lastUTMGridConvergence = convergence; +} + +void CompassConverter::setMagneticModelPath(const cras::optional& modelPath) +{ + this->data->magneticModelManager->setModelPath(modelPath); +} + +void CompassConverter::forceMagneticModelName(const std::string& model) +{ + this->forcedMagneticModelName = model; +} + +void CompassConverter::setKeepUTMZone(const bool keep) +{ + this->keepUTMZone = keep; +} + +cras::expected CompassConverter::getMagneticDeclination(const ros::Time& stamp) const +{ + if (this->forcedMagneticDeclination.has_value()) + return *this->forcedMagneticDeclination; + + if (!this->lastFix.has_value()) + return cras::make_unexpected("Cannot determine magnetic declination without GNSS pose."); + + return this->computeMagneticDeclination(*this->lastFix, stamp); +} + +cras::expected CompassConverter::computeMagneticDeclination( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const +{ + const auto year = cras::getYear(stamp); + if (this->data->magneticModels[year] == nullptr) + { + const auto modelName = !this->forcedMagneticModelName.empty() ? + this->forcedMagneticModelName : this->data->magneticModelManager->getBestMagneticModelName(stamp); + + const auto model = this->data->magneticModelManager->getMagneticModel(modelName, this->strict); + if (!model.has_value()) + return cras::make_unexpected(cras::format( + "Could not create magnetic field model %s for year %u because of the following error: %s", + modelName.c_str(), year, model.error().c_str())); + this->data->magneticModels[year] = *model; + } + + const auto& magModel = *this->data->magneticModels[year]; + + const auto fieldComponents = magModel.getMagneticFieldComponents(fix, stamp); + if (!fieldComponents.has_value()) + return cras::make_unexpected(fieldComponents.error()); + + return fieldComponents->values.declination; +} + +cras::expected CompassConverter::getUTMGridConvergence() const +{ + if (this->forcedUTMGridConvergence.has_value()) + return *this->forcedUTMGridConvergence; + + if (!this->lastUTMGridConvergence.has_value()) + return cras::make_unexpected("UTM grid convergence has not yet been determined from GNSS pose."); + + return *this->lastUTMGridConvergence; +} + +cras::expected CompassConverter::getUTMZone() const +{ + if (this->forcedUTMZone.has_value()) + return *this->forcedUTMZone; + + if (!this->lastUTMZone.has_value()) + return cras::make_unexpected("UTM zone has not yet been determined from GNSS pose."); + + return *this->lastUTMZone; +} + +void CompassConverter::forceUTMZone(const cras::optional& zone) +{ + if (zone.has_value() && (zone < GeographicLib::UTMUPS::MINZONE || zone > GeographicLib::UTMUPS::MAXZONE)) + CRAS_WARN("Invalid UTM zone: %d", *zone); + else + this->forcedUTMZone = this->lastUTMZone = zone; +} + +cras::expected, std::string> CompassConverter::computeUTMGridConvergenceAndZone( + const sensor_msgs::NavSatFix& fix, const cras::optional& utmZone) const +{ + if (utmZone.has_value() && (*utmZone < GeographicLib::UTMUPS::MINZONE || *utmZone > GeographicLib::UTMUPS::MAXZONE)) + return cras::make_unexpected(cras::format("Invalid UTM zone: %d", *utmZone)); + + try + { + int zone; + bool isNorthHemisphere; + double northing, easting, utmGridConvergence, projectionScale; + const int setzone = utmZone.value_or(GeographicLib::UTMUPS::STANDARD); + + GeographicLib::UTMUPS::Forward(fix.latitude, fix.longitude, + zone, isNorthHemisphere, easting, northing, utmGridConvergence, projectionScale, setzone); + + return std::make_pair(angles::from_degrees(utmGridConvergence), zone); + } + catch (const GeographicLib::GeographicErr& e) + { + return cras::make_unexpected(cras::format("Could not get UTM grid convergence: %s", e.what())); + } +} + +cras::expected CompassConverter::convertAzimuth( + const compass_msgs::Azimuth& azimuth, + const decltype(compass_msgs::Azimuth::unit) unit, + const decltype(compass_msgs::Azimuth::orientation) orientation, + const decltype(compass_msgs::Azimuth::reference) reference) const +{ + // Fast track for no conversion + if (azimuth.unit == unit && azimuth.orientation == orientation && azimuth.reference == reference) + return azimuth; + + compass_msgs::Azimuth result = azimuth; + result.unit = unit; + result.orientation = orientation; + result.reference = reference; + + using Az = compass_msgs::Azimuth; + + // Convert the input to NED radians + if (azimuth.unit == Az::UNIT_DEG) + result.azimuth = angles::from_degrees(result.azimuth); + if (azimuth.orientation == Az::ORIENTATION_ENU) + result.azimuth = M_PI_2 - result.azimuth; + + // When going magnetic->true, we need to add declination in NED. + // When going true->UTM, we need to subtract grid convergence in NED. + + // Now convert between references in NED radians + if (azimuth.reference != result.reference) + { + if (azimuth.reference == Az::REFERENCE_MAGNETIC) + { + const auto magneticDeclination = this->getMagneticDeclination(azimuth.header.stamp); + if (!magneticDeclination.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert magnetic azimuth to true without knowing magnetic declination. Error: %s", + magneticDeclination.error().c_str())); + + result.azimuth += *magneticDeclination; + + if (result.reference == Az::REFERENCE_UTM) + { + const auto convergence = this->getUTMGridConvergence(); + if (!convergence.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert true azimuth to UTM without knowing UTM grid convergence. Error: %s", + convergence.error().c_str())); + + result.azimuth -= *convergence; + } + } + else if (azimuth.reference == Az::REFERENCE_GEOGRAPHIC) + { + if (result.reference == Az::REFERENCE_MAGNETIC) + { + const auto magneticDeclination = this->getMagneticDeclination(azimuth.header.stamp); + if (!magneticDeclination.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert true azimuth to magnetic without knowing magnetic declination. Error: %s", + magneticDeclination.error().c_str())); + + result.azimuth -= *magneticDeclination; + } + else if (result.reference == Az::REFERENCE_UTM) + { + const auto convergence = this->getUTMGridConvergence(); + if (!convergence.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert true azimuth to UTM without knowing UTM grid convergence. Error: %s", + convergence.error().c_str())); + + result.azimuth -= *convergence; + } + } + else if (azimuth.reference == Az::REFERENCE_UTM) + { + const auto convergence = this->getUTMGridConvergence(); + if (!convergence.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert UTM azimuth to true without knowing UTM grid convergence. Error: %s", + convergence.error().c_str())); + + result.azimuth += *convergence; + + if (result.reference == Az::REFERENCE_MAGNETIC) + { + const auto magneticDeclination = this->getMagneticDeclination(azimuth.header.stamp); + if (!magneticDeclination.has_value()) + return cras::make_unexpected(cras::format( + "Cannot convert true azimuth to magnetic without knowing magnetic declination. Error: %s", + magneticDeclination.error().c_str())); + + result.azimuth -= *magneticDeclination; + } + } + } + + // Reference is correct now; convert to the output unit and orientation + if (result.orientation == Az::ORIENTATION_ENU) + result.azimuth = M_PI_2 - result.azimuth; + result.azimuth = angles::normalize_angle_positive(result.azimuth); + if (result.unit == Az::UNIT_DEG) + result.azimuth = angles::to_degrees(result.azimuth); + + if (azimuth.unit == Az::UNIT_RAD && result.unit == Az::UNIT_DEG) + result.variance = std::pow(angles::to_degrees(std::sqrt(azimuth.variance)), 2); + else if (azimuth.unit == Az::UNIT_DEG && result.unit == Az::UNIT_RAD) + result.variance = std::pow(angles::from_degrees(std::sqrt(azimuth.variance)), 2); + + return result; +} + +cras::expected CompassConverter::convertQuaternion( + const geometry_msgs::QuaternionStamped& quat, + const decltype(compass_msgs::Azimuth::variance) variance, + const decltype(compass_msgs::Azimuth::unit) unit, + const decltype(compass_msgs::Azimuth::orientation) orientation, + const decltype(compass_msgs::Azimuth::reference) reference) const +{ + return this->convertQuaternion(quat.quaternion, quat.header, variance, unit, orientation, reference); +} + +cras::expected CompassConverter::convertQuaternion( + const geometry_msgs::Quaternion& quat, + const std_msgs::Header& header, + const decltype(compass_msgs::Azimuth::variance) variance, + const decltype(compass_msgs::Azimuth::unit) unit, + const decltype(compass_msgs::Azimuth::orientation) orientation, + const decltype(compass_msgs::Azimuth::reference) reference) const +{ + tf2::Quaternion q; + tf2::fromMsg(quat, q); + if (q.length2() < 1e-6) + return cras::make_unexpected("Invalid quaternion (all zeros)."); + + compass_msgs::Azimuth result; + result.header = header; + result.azimuth = cras::getYaw(quat); + result.variance = variance; + if (unit == Az::UNIT_DEG) + { + result.azimuth = angles::to_degrees(result.azimuth); + result.variance = std::pow(angles::to_degrees(std::sqrt(variance)), 2); + } + result.orientation = orientation; + result.unit = unit; + result.reference = reference; + return result; +} + +cras::expected CompassConverter::convertToQuaternion( + const compass_msgs::Azimuth& azimuth) const +{ + tf2::Stamped quat; + quat.frame_id_ = azimuth.header.frame_id; + quat.stamp_ = azimuth.header.stamp; + quat.setRPY(0, 0, azimuth.azimuth * (azimuth.unit == Az::UNIT_RAD ? 1 : M_PI / 180.0)); + return tf2::toMsg(quat); +} + +cras::expected CompassConverter::convertToPose( + const compass_msgs::Azimuth& azimuth) const +{ + const auto maybeQuat = this->convertToQuaternion(azimuth); + if (!maybeQuat.has_value()) + return cras::make_unexpected(cras::format("Could not convert azimuth to pose: %s", maybeQuat.error().c_str())); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header = azimuth.header; + pose.pose.pose.orientation = maybeQuat->quaternion; + pose.pose.covariance[0 * 6 + 0] = std::numeric_limits::infinity(); + pose.pose.covariance[1 * 6 + 1] = std::numeric_limits::infinity(); + pose.pose.covariance[2 * 6 + 2] = std::numeric_limits::infinity(); + pose.pose.covariance[3 * 6 + 3] = 4 * M_PI * M_PI; + pose.pose.covariance[4 * 6 + 4] = 4 * M_PI * M_PI; + pose.pose.covariance[5 * 6 + 5] = azimuth.variance; + + return pose; +} + +cras::expected CompassConverter::convertToImu(const compass_msgs::Azimuth& azimuth) const +{ + const auto maybeQuat = this->convertToQuaternion(azimuth); + if (!maybeQuat.has_value()) + return cras::make_unexpected(cras::format("Could not convert azimuth to pose: %s", maybeQuat.error().c_str())); + + sensor_msgs::Imu imu; + imu.header = azimuth.header; + imu.linear_acceleration_covariance[0] = -1; + imu.angular_velocity_covariance[0] = -1; + imu.orientation = maybeQuat->quaternion; + imu.orientation_covariance[0 * 3 + 0] = 4 * M_PI * M_PI; + imu.orientation_covariance[1 * 3 + 1] = 4 * M_PI * M_PI; + imu.orientation_covariance[2 * 3 + 2] = azimuth.variance; + + return imu; +} + +cras::expected CompassConverter::convertQuaternionMsgEvent( + const ros::MessageEvent& quatEvent, + const decltype(compass_msgs::Azimuth::variance) variance, + const decltype(compass_msgs::Azimuth::unit) unit, + const cras::optional& orientation, + const cras::optional& reference) const +{ + auto msgOrientation = orientation; + auto msgReference = reference; + if (!msgOrientation.has_value() || !msgReference.has_value()) + { + const auto maybeAzimuthParams = parseAzimuthTopicName(quatEvent.getConnectionHeaderPtr()); + if (maybeAzimuthParams.has_value()) + { + msgOrientation = std::get<1>(*maybeAzimuthParams); + msgReference = std::get<2>(*maybeAzimuthParams); + } + } + + if (!msgOrientation.has_value() || !msgReference.has_value()) + return cras::make_unexpected("Orientation and reference are not specified and cannot be autodetected."); + + const auto msg = quatEvent.getConstMessage(); + return this->convertQuaternion(*msg, variance, unit, *msgOrientation, *msgReference); +} + +cras::expected CompassConverter::convertPoseMsgEvent( + const ros::MessageEvent& poseEvent, + const decltype(compass_msgs::Azimuth::unit) unit, + const cras::optional& orientation, + const cras::optional& reference) const +{ + auto msgOrientation = orientation; + auto msgReference = reference; + if (!msgOrientation.has_value() || !msgReference.has_value()) + { + const auto maybeAzimuthParams = parseAzimuthTopicName(poseEvent.getConnectionHeaderPtr()); + if (maybeAzimuthParams.has_value()) + { + msgOrientation = std::get<1>(*maybeAzimuthParams); + msgReference = std::get<2>(*maybeAzimuthParams); + } + } + + if (!msgOrientation.has_value() || !msgReference.has_value()) + return cras::make_unexpected("Orientation and reference are not specified and cannot be autodetected."); + + const auto msg = poseEvent.getConstMessage(); + return this->convertQuaternion( + msg->pose.pose.orientation, msg->header, msg->pose.covariance[5 * 6 + 5], unit, *msgOrientation, *msgReference); +} + +cras::expected CompassConverter::convertImuMsgEvent( + const ros::MessageEvent& imuEvent, + const decltype(compass_msgs::Azimuth::unit) unit, + const cras::optional& orientation, + const cras::optional& reference) const +{ + auto msgOrientation = orientation; + auto msgReference = reference; + if (!msgOrientation.has_value() || !msgReference.has_value()) + { + const auto maybeAzimuthParams = parseAzimuthTopicName(imuEvent.getConnectionHeaderPtr()); + if (maybeAzimuthParams.has_value()) + { + msgOrientation = std::get<1>(*maybeAzimuthParams); + msgReference = std::get<2>(*maybeAzimuthParams); + } + } + + // IMUs should output orientation in ENU frame + if (!msgOrientation.has_value()) + msgOrientation = Az::ORIENTATION_ENU; + + if (!msgReference.has_value()) + return cras::make_unexpected("Reference is not specified and cannot be autodetected."); + + const auto msg = imuEvent.getConstMessage(); + return this->convertQuaternion( + msg->orientation, msg->header, msg->orientation_covariance[2 * 3 + 2], unit, *msgOrientation, *msgReference); +} + +template using ME = ros::MessageEvent; +template using Creator = ros::DefaultMessageCreator; + +cras::expected CompassConverter::convertUniversalMsgEvent( + const ros::MessageEvent& event, + const decltype(compass_msgs::Azimuth::variance) variance, + const decltype(compass_msgs::Azimuth::unit) unit, + const cras::optional& orientation, + const cras::optional& reference) const +{ + const auto msg = event.getConstMessage(); + const auto header = event.getConnectionHeaderPtr(); + const auto stamp = event.getReceiptTime(); + const auto& type = msg->getDataType(); + namespace mt = ros::message_traits; + + if (type == mt::datatype()) + { + const auto az = *msg->instantiate(); + return this->convertAzimuth(az, unit, az.orientation, az.reference); + } + else if (type == mt::datatype()) + { + const auto poseEvent = ME(msg->instantiate(), header, stamp, false, Creator()); + return this->convertPoseMsgEvent(poseEvent, unit, orientation, reference); + } + else if (type == mt::datatype()) + { + const auto quatEvent = ME(msg->instantiate(), header, stamp, false, Creator()); + return this->convertQuaternionMsgEvent(quatEvent, variance, unit, orientation, reference); + } + else if (type == mt::datatype()) + { + const auto imuEvent = ME(msg->instantiate(), header, stamp, false, Creator()); + return this->convertImuMsgEvent(imuEvent, unit, orientation, reference); + } + else + return cras::make_unexpected(cras::format("Invalid message type: %s.", type.c_str())); +} + +void CompassConverter::setNavSatPos(const sensor_msgs::NavSatFix& fix) +{ + this->lastFix = fix; + + if (!this->forcedUTMGridConvergence.has_value()) + { + const auto maybeConvergenceAndZone = this->computeUTMGridConvergenceAndZone(fix, this->forcedUTMZone); + if (!maybeConvergenceAndZone.has_value()) + { + CRAS_WARN_THROTTLE(10.0, "Error computing UTM grid convergence: %s", maybeConvergenceAndZone.error().c_str()); + } + else + { + const auto [convergence, zone] = *maybeConvergenceAndZone; + + this->lastUTMZone = zone; + if (this->keepUTMZone && !this->forcedUTMZone.has_value()) + this->forcedUTMZone = zone; + + this->lastUTMGridConvergence = convergence; + } + } +} + +} diff --git a/compass_conversions/src/message_filter.cpp b/compass_conversions/src/message_filter.cpp new file mode 100644 index 0000000..1ff9aae --- /dev/null +++ b/compass_conversions/src/message_filter.cpp @@ -0,0 +1,166 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Message filter to convert between various compass representations. + * \author Martin Pecka + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +namespace compass_conversions +{ + +UniversalAzimuthSubscriber::UniversalAzimuthSubscriber(const cras::LogHelperPtr& log, ros::NodeHandle& nh, + const std::string& topic, const uint32_t queueSize, const ros::TransportHints& transportHints, + ros::CallbackQueueInterface* callbackQueue) : cras::HasLogger(log), converter(log, true) +{ + this->subscribe(nh, topic, queueSize, transportHints, callbackQueue); +} + +UniversalAzimuthSubscriber::~UniversalAzimuthSubscriber() +{ + this->unsubscribe(); +} + +void UniversalAzimuthSubscriber::subscribe(ros::NodeHandle& nh, const std::string& topic, const uint32_t queueSize, + const ros::TransportHints& transportHints, ros::CallbackQueueInterface* callbackQueue) +{ + this->unsubscribe(); + + if (!topic.empty()) + { + this->subscribeOps.template initByFullCallbackType( + topic, queueSize, boost::bind(&UniversalAzimuthSubscriber::cb, this, boost::placeholders::_1)); + this->subscribeOps.callback_queue = callbackQueue; + this->subscribeOps.transport_hints = transportHints; + this->sub = nh.subscribe(this->subscribeOps); + this->nh = nh; + } +} + +void UniversalAzimuthSubscriber::subscribe() +{ + this->unsubscribe(); + + if (!this->subscribeOps.topic.empty()) + this->sub = this->nh.subscribe(this->subscribeOps); +} + +void UniversalAzimuthSubscriber::unsubscribe() +{ + sub.shutdown(); +} + +void UniversalAzimuthSubscriber::setInputDefaults( + const cras::optional& orientation, + const cras::optional& reference, + const cras::optional& variance) +{ + this->inputOrientation = orientation; + this->inputReference = reference; + this->inputVariance = variance; +} + +void UniversalAzimuthSubscriber::configFromParams(const cras::BoundParamHelper& params) +{ + cras::optional inputOrientation; + if (params.hasParam("input_orientation")) + inputOrientation = params.getParam( + "input_orientation", cras::nullopt, "", + {.resultToStr = &compass_msgs::orientationToString, .toResult = &compass_msgs::parseOrientation}); + + cras::optional inputReference; + if (params.hasParam("input_reference")) + inputReference = params.getParam( + "input_reference", cras::nullopt, "", + {.resultToStr = &compass_msgs::referenceToString, .toResult = &compass_msgs::parseReference}); + + cras::optional inputVariance; + if (params.hasParam("input_variance")) + inputVariance = params.getParam("input_variance", cras::nullopt, "rad^2"); + + this->setInputDefaults(inputOrientation, inputReference, inputVariance); +} + +std::string UniversalAzimuthSubscriber::getTopic() const +{ + return this->subscribeOps.topic; +} + +const ros::Subscriber& UniversalAzimuthSubscriber::getSubscriber() const +{ + return this->sub; +} + +void UniversalAzimuthSubscriber::add(const EventType&) +{ +} + +void UniversalAzimuthSubscriber::cb(const EventType& event) +{ + const auto maybeAzimuth = this->converter.convertUniversalMsgEvent( + event, this->inputVariance.value_or(0.0), Az::UNIT_RAD, this->inputOrientation, this->inputReference); + if (!maybeAzimuth.has_value()) + { + CRAS_ERROR_THROTTLE(10.0, "Error converting message to Azimuth: %s", maybeAzimuth.error().c_str()); + return; + } + + const auto header = event.getConnectionHeaderPtr(); + const auto stamp = event.getReceiptTime(); + this->signalMessage(ros::MessageEvent( + boost::make_shared(*maybeAzimuth), header, stamp, false, ros::DefaultMessageCreator())); +} + + +CompassFilter::~CompassFilter() = default; + +void CompassFilter::cbAzimuth(const AzimuthEventType& azimuthEvent) +{ + const auto& msg = azimuthEvent.getConstMessage(); + const auto output = this->converter->convertAzimuth( + *msg, this->unit, this->orientation, this->reference.value_or(msg->reference)); + if (!output.has_value()) + { + CRAS_ERROR_THROTTLE(10.0, + "Azimuth conversion failed%s: %s", fixReceived ? "" : "(no fix message received yet)", output.error().c_str()); + return; + } + this->signalMessage(AzimuthEventType( + boost::make_shared(*output), azimuthEvent.getConnectionHeaderPtr(), + azimuthEvent.getReceiptTime(), false, ros::DefaultMessageCreator())); +} + +void CompassFilter::cbFix(const FixEventType& fixEvent) +{ + this->fixReceived = true; + this->converter->setNavSatPos(*fixEvent.getConstMessage()); +} + +void CompassFilter::cbUTMZone(const UTMZoneEventType& utmZoneEvent) +{ + this->converter->forceUTMZone(utmZoneEvent.getConstMessage()->data); +} + +} diff --git a/compass_conversions/src/tf2_compass_msgs.cpp b/compass_conversions/src/tf2_compass_msgs.cpp new file mode 100644 index 0000000..e94af6d --- /dev/null +++ b/compass_conversions/src/tf2_compass_msgs.cpp @@ -0,0 +1,60 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Support for transforming compass_msgs::Azimuth messages. + * \author Martin Pecka + */ + +#include + +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +namespace tf2 +{ + +template<> +const ros::Time& getTimestamp(const compass_msgs::Azimuth& t) +{ + return t.header.stamp; +} + +template<> +const std::string& getFrameId(const compass_msgs::Azimuth& t) +{ + return t.header.frame_id; +} + +compass_msgs::Azimuth toMsg(const compass_msgs::Azimuth& in) +{ + return in; +} + +void fromMsg(const compass_msgs::Azimuth& msg, compass_msgs::Azimuth& out) +{ + out = msg; +} + +template<> +void doTransform( + const compass_msgs::Azimuth& t_in, compass_msgs::Azimuth& t_out, const geometry_msgs::TransformStamped& transform) +{ + t_out = t_in; + t_out.header.frame_id = transform.header.frame_id; + t_out.header.stamp = transform.header.stamp; + + auto yaw = cras::getYaw(transform.transform.rotation); + if (t_in.unit == Az::UNIT_DEG) + yaw = angles::to_degrees(yaw); + + t_out.azimuth += yaw * (t_in.orientation == Az::ORIENTATION_NED ? -1 : 1); +} + +} diff --git a/compass_conversions/src/topic_names.cpp b/compass_conversions/src/topic_names.cpp new file mode 100644 index 0000000..5931c01 --- /dev/null +++ b/compass_conversions/src/topic_names.cpp @@ -0,0 +1,126 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Logic for naming topics according to the type of Azimuth message they carry. + * \author Martin Pecka + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +namespace compass_conversions +{ + +namespace +{ + +std::string getAzimuthTopicSuffix(const decltype(Az::orientation) orientation, const decltype(Az::reference) reference) +{ + const std::string refStr = + reference == Az::REFERENCE_MAGNETIC ? "mag" : (reference == Az::REFERENCE_GEOGRAPHIC ? "true" : "utm"); + const std::string orStr = orientation == Az::ORIENTATION_ENU ? "enu" : "ned"; + return refStr + "/" + orStr; +} + +} + +template<> std::string getAzimuthTopicSuffix( + const decltype(Az::unit) unit, + const decltype(Az::orientation) orientation, + const decltype(Az::reference) reference) +{ + const auto unitStr = unit == Az::UNIT_RAD ? "rad" : "deg"; + return getAzimuthTopicSuffix(orientation, reference) + "/" + unitStr; +} + +template<> std::string getAzimuthTopicSuffix( + const decltype(Az::unit) unit, + const decltype(Az::orientation) orientation, + const decltype(Az::reference) reference) +{ + return getAzimuthTopicSuffix(orientation, reference) + "/quat"; +} + +template<> std::string getAzimuthTopicSuffix( + const decltype(Az::unit) unit, + const decltype(Az::orientation) orientation, + const decltype(Az::reference) reference) +{ + return getAzimuthTopicSuffix(orientation, reference) + "/pose"; +} + +template<> std::string getAzimuthTopicSuffix( + const decltype(Az::unit) unit, + const decltype(Az::orientation) orientation, + const decltype(Az::reference) reference) +{ + return getAzimuthTopicSuffix(orientation, reference) + "/imu"; +} + +cras::optional> +parseAzimuthTopicName(const std::string& topic) +{ + const auto parts = cras::split(topic, "/"); + if (parts.size() < 3) + return cras::nullopt; + + auto it = parts.rbegin(); + const auto unitPart = *it; + ++it; + const auto orPart = *it; + ++it; + const auto refPart = *it; + + decltype(Az::unit) unit; + if (unitPart == "deg") + unit = Az::UNIT_DEG; + else if (unitPart == "rad" || unitPart == "imu" || unitPart == "pose" || unitPart == "quat") + unit = Az::UNIT_RAD; + else + return cras::nullopt; + + decltype(Az::orientation) orientation; + if (orPart == "ned") + orientation = Az::ORIENTATION_NED; + else if (orPart == "enu") + orientation = Az::ORIENTATION_ENU; + else + return cras::nullopt; + + decltype(Az::reference) reference; + if (refPart == "mag") + reference = Az::REFERENCE_MAGNETIC; + else if (refPart == "true") + reference = Az::REFERENCE_GEOGRAPHIC; + else if (refPart == "utm") + reference = Az::REFERENCE_UTM; + else + return cras::nullopt; + + return std::make_tuple(unit, orientation, reference); +} + +cras::optional> +parseAzimuthTopicName(const boost::shared_ptr& connectionHeaderPtr) +{ + if (connectionHeaderPtr != nullptr && connectionHeaderPtr->find("topic") != connectionHeaderPtr->end()) + return parseAzimuthTopicName(connectionHeaderPtr->at("topic")); + return cras::nullopt; +} + +} diff --git a/compass_conversions/test/test_compass_converter.cpp b/compass_conversions/test/test_compass_converter.cpp new file mode 100644 index 0000000..f662831 --- /dev/null +++ b/compass_conversions/test/test_compass_converter.cpp @@ -0,0 +1,1574 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for CompassConverter. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +TEST(CompassConverter, Construct) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + ASSERT_NO_THROW(compass_conversions::CompassConverter converter(log, true)); + ASSERT_NO_THROW(compass_conversions::CompassConverter converter(log, false)); +} + +TEST(CompassConverter, ConfigFromParams) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + XmlRpc::XmlRpcValue params; + params.begin(); // Convert to struct + cras::BoundParamHelper paramHelper(log, std::make_shared(params, "")); + + converter.configFromParams(paramHelper); + + params["magnetic_declination"] = 1.0; + paramHelper = {log, std::make_shared(params, "")}; + converter.configFromParams(paramHelper); + + params["utm_grid_convergence"] = 2.0; + paramHelper = {log, std::make_shared(params, "")}; + converter.configFromParams(paramHelper); + + params.clear(); + params["initial_lat"] = 0.0; + params["initial_lon"] = 0.0; + paramHelper = {log, std::make_shared(params, "")}; + converter.configFromParams(paramHelper); + + params["alt"] = 0.0; + paramHelper = {log, std::make_shared(params, "")}; + converter.configFromParams(paramHelper); +} + +TEST(CompassConverter, ComputeMagneticDeclination) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + auto time = cras::parseTime("2024-11-18T13:00:00Z"); + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 15.0; + fix.altitude = 200.0; + auto maybeDeclination = converter.computeMagneticDeclination(fix, time); + if (!maybeDeclination.has_value()) + ROS_ERROR("%s", maybeDeclination.error().c_str()); + ASSERT_TRUE(maybeDeclination.has_value()); + EXPECT_NEAR(5.333, angles::to_degrees(*maybeDeclination), 1e-3); + + time = cras::parseTime("2019-11-18T13:00:00Z"); + maybeDeclination = converter.computeMagneticDeclination(fix, time); + if (!maybeDeclination.has_value()) + ROS_ERROR("%s", maybeDeclination.error().c_str()); + ASSERT_TRUE(maybeDeclination.has_value()); + EXPECT_NEAR(4.507, angles::to_degrees(*maybeDeclination), 1e-3); + + // No magnetic model for 2000 + time = cras::parseTime("2000-11-18T13:00:00Z"); + maybeDeclination = converter.computeMagneticDeclination(fix, time); + EXPECT_FALSE(maybeDeclination.has_value()); +} + +TEST(CompassConverter, GetMagneticDeclination) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + auto time = cras::parseTime("2024-11-18T13:00:00Z"); + + auto maybeDeclination = converter.getMagneticDeclination(time); + EXPECT_FALSE(maybeDeclination.has_value()); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 15.0; + fix.altitude = 200.0; + converter.setNavSatPos(fix); + + maybeDeclination = converter.getMagneticDeclination(time); + if (!maybeDeclination.has_value()) + ROS_ERROR("%s", maybeDeclination.error().c_str()); + ASSERT_TRUE(maybeDeclination.has_value()); + EXPECT_NEAR(5.333, angles::to_degrees(*maybeDeclination), 1e-3); + + time = cras::parseTime("2019-11-18T13:00:00Z"); + maybeDeclination = converter.getMagneticDeclination(time); + if (!maybeDeclination.has_value()) + ROS_ERROR("%s", maybeDeclination.error().c_str()); + ASSERT_TRUE(maybeDeclination.has_value()); + EXPECT_NEAR(4.507, angles::to_degrees(*maybeDeclination), 1e-3); + + // No magnetic model for 2000 + time = cras::parseTime("2000-11-18T13:00:00Z"); + maybeDeclination = converter.getMagneticDeclination(time); + EXPECT_FALSE(maybeDeclination.has_value()); +} + +TEST(CompassConverter, ComputeUTMGridConvergence) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 15.0; + auto maybeConvergenceAndZone = converter.computeUTMGridConvergenceAndZone(fix, cras::nullopt); + ASSERT_TRUE(maybeConvergenceAndZone.has_value()); + EXPECT_NEAR(0, angles::to_degrees(maybeConvergenceAndZone->first), 1e-3); + EXPECT_EQ(33, maybeConvergenceAndZone->second); + + fix.latitude = 51.0; + fix.longitude = 10.0; + maybeConvergenceAndZone = converter.computeUTMGridConvergenceAndZone(fix, cras::nullopt); + ASSERT_TRUE(maybeConvergenceAndZone.has_value()); + EXPECT_NEAR(0.777177, angles::to_degrees(maybeConvergenceAndZone->first), 1e-5); + EXPECT_EQ(32, maybeConvergenceAndZone->second); + + fix.latitude = -51.0; + fix.longitude = 10.0; + maybeConvergenceAndZone = converter.computeUTMGridConvergenceAndZone(fix, cras::nullopt); + ASSERT_TRUE(maybeConvergenceAndZone.has_value()); + EXPECT_NEAR(-0.777177, angles::to_degrees(maybeConvergenceAndZone->first), 1e-5); + EXPECT_EQ(32, maybeConvergenceAndZone->second); + + // Force the neighbor zone (this should be zone 32). + fix.latitude = 51.0; + fix.longitude = 10.0; + maybeConvergenceAndZone = converter.computeUTMGridConvergenceAndZone(fix, 33); + ASSERT_TRUE(maybeConvergenceAndZone.has_value()); + EXPECT_NEAR(-3.8896687, angles::to_degrees(maybeConvergenceAndZone->first), 1e-5); + EXPECT_EQ(33, maybeConvergenceAndZone->second); +} + +TEST(CompassConverter, GetUTMGridConvergence) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + converter.setKeepUTMZone(false); + + auto maybeConvergence = converter.getUTMGridConvergence(); + auto maybeZone = converter.getUTMZone(); + EXPECT_FALSE(maybeConvergence.has_value()); + EXPECT_FALSE(maybeZone.has_value()); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 15.0; + converter.setNavSatPos(fix); + maybeConvergence = converter.getUTMGridConvergence(); + maybeZone = converter.getUTMZone(); + ASSERT_TRUE(maybeConvergence.has_value()); + EXPECT_NEAR(0, angles::to_degrees(*maybeConvergence), 1e-3); + EXPECT_EQ(33, *maybeZone); + + fix.latitude = 51.0; + fix.longitude = 10.0; + converter.setNavSatPos(fix); + maybeConvergence = converter.getUTMGridConvergence(); + maybeZone = converter.getUTMZone(); + ASSERT_TRUE(maybeConvergence.has_value()); + EXPECT_NEAR(0.777177, angles::to_degrees(*maybeConvergence), 1e-5); + EXPECT_EQ(32, *maybeZone); + + fix.latitude = -51.0; + fix.longitude = 10.0; + converter.setNavSatPos(fix); + maybeConvergence = converter.getUTMGridConvergence(); + maybeZone = converter.getUTMZone(); + ASSERT_TRUE(maybeConvergence.has_value()); + EXPECT_NEAR(-0.777177, angles::to_degrees(*maybeConvergence), 1e-5); + EXPECT_EQ(32, *maybeZone); + + // Force the neighbor zone (this should be zone 32). + fix.latitude = 51.0; + fix.longitude = 10.0; + converter.forceUTMZone(33); + converter.setNavSatPos(fix); + maybeConvergence = converter.getUTMGridConvergence(); + maybeZone = converter.getUTMZone(); + ASSERT_TRUE(maybeConvergence.has_value()); + EXPECT_NEAR(-3.8896687, angles::to_degrees(*maybeConvergence), 1e-5); + EXPECT_EQ(33, *maybeZone); +} + +TEST(CompassConverter, ConvertNotRequiresNavSat) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + for (const auto reference : std::list{Az::REFERENCE_MAGNETIC, Az::REFERENCE_GEOGRAPHIC, Az::REFERENCE_UTM}) + { + SCOPED_TRACE(reference); + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = reference; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, reference); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_EQ(M_PI_2, maybeAzimuth->azimuth); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(reference, maybeAzimuth->reference); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = reference; + azimuth.azimuth = M_PI_2; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, reference); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90, maybeAzimuth->azimuth, 1e-9); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(reference, maybeAzimuth->reference); + + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = reference; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, reference); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2, maybeAzimuth->azimuth, 1e-9); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(reference, maybeAzimuth->reference); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = reference; + azimuth.azimuth = M_PI_2; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, reference); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(0, maybeAzimuth->azimuth, 1e-9); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(reference, maybeAzimuth->reference); + + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = reference; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, reference); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(0, maybeAzimuth->azimuth, 1e-9); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(reference, maybeAzimuth->reference); + } +} + +TEST(CompassConverter, ConvertNavSatMissing) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + azimuth.azimuth = M_PI_2; + + for (const auto fromUnit : std::list{Az::UNIT_DEG, Az::UNIT_RAD}) + { + SCOPED_TRACE(fromUnit); + for (const auto toUnit : std::list{Az::UNIT_DEG, Az::UNIT_RAD}) + { + SCOPED_TRACE(toUnit); + for (const auto fromOrientation : std::list{Az::ORIENTATION_ENU, Az::ORIENTATION_NED}) + { + SCOPED_TRACE(fromOrientation); + for (const auto toOrientation : std::list{Az::ORIENTATION_ENU, Az::ORIENTATION_NED}) + { + SCOPED_TRACE(toOrientation); + + azimuth.unit = fromUnit; azimuth.orientation = fromOrientation; + + azimuth.reference = Az::REFERENCE_MAGNETIC; + auto maybeAzimuth = converter.convertAzimuth(azimuth, toUnit, toOrientation, Az::REFERENCE_GEOGRAPHIC); + EXPECT_FALSE(maybeAzimuth.has_value()); + + azimuth.reference = Az::REFERENCE_UTM; + maybeAzimuth = converter.convertAzimuth(azimuth, toUnit, toOrientation, Az::REFERENCE_GEOGRAPHIC); + EXPECT_FALSE(maybeAzimuth.has_value()); + + azimuth.reference = Az::REFERENCE_MAGNETIC; + maybeAzimuth = converter.convertAzimuth(azimuth, toUnit, toOrientation, Az::REFERENCE_UTM); + EXPECT_FALSE(maybeAzimuth.has_value()); + + azimuth.reference = Az::REFERENCE_GEOGRAPHIC; + maybeAzimuth = converter.convertAzimuth(azimuth, toUnit, toOrientation, Az::REFERENCE_UTM); + EXPECT_FALSE(maybeAzimuth.has_value()); + } + } + } + } +} + +TEST(CompassConverter, ConvertRequiresNavSatFromMag) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + sensor_msgs::NavSatFix fix; + fix.header = azimuth.header; + fix.latitude = 51; + fix.longitude = 10; + fix.altitude = 200; + + double declinationDeg = 4.04; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 0.777177; + double convergenceRad = angles::from_degrees(convergenceDeg); + + converter.setNavSatPos(fix); + + // + // From: MAG, RAD, ENU + // + + // To: MAG->GEO, RAD, ENU + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_MAGNETIC; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->UTM, RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad + convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg + convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // + // From: MAG, RAD, NED + // + + // To: MAG->GEO, RAD, NED + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_MAGNETIC; + azimuth.azimuth = M_PI_2; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->UTM, RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad - convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg - convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + + // + // From: MAG, DEG, ENU + // + + // To: MAG->GEO, DEG, ENU + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_MAGNETIC; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->UTM, DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg + convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad + convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // + // From: MAG, DEG, NED + // + + // To: MAG->GEO, DEG, NED + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_MAGNETIC; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->GEO, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: MAG->UTM, DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg - convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: MAG->UTM, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad - convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertRequiresNavSatFromGeo) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + sensor_msgs::NavSatFix fix; + fix.header = azimuth.header; + fix.latitude = 51; + fix.longitude = 10; + fix.altitude = 200; + + double declinationDeg = 4.04; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 0.777177; + double convergenceRad = angles::from_degrees(convergenceDeg); + + converter.setNavSatPos(fix); + + // + // From: GEO, RAD, ENU + // + + // To: GEO->MAG, RAD, ENU + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_GEOGRAPHIC; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->UTM, RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // + // From: GEO, RAD, NED + // + + // To: GEO->MAG, RAD, NED + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_GEOGRAPHIC; + azimuth.azimuth = M_PI_2; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->UTM, RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + + // + // From: GEO, DEG, ENU + // + + // To: GEO->MAG, DEG, ENU + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_GEOGRAPHIC; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->UTM, DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // + // From: GEO, DEG, NED + // + + // To: GEO->MAG, DEG, NED + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_GEOGRAPHIC; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->MAG, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: GEO->UTM, DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); + + // To: GEO->UTM, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertRequiresNavSatFromUTM) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + sensor_msgs::NavSatFix fix; + fix.header = azimuth.header; + fix.latitude = 51; + fix.longitude = 10; + fix.altitude = 200; + + double declinationDeg = 4.04; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 0.777177; + double convergenceRad = angles::from_degrees(convergenceDeg); + + converter.setNavSatPos(fix); + + // + // From: UTM, RAD, ENU + // + + // To: UTM->MAG, RAD, ENU + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad - convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg - convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD->DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD->DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // + // From: UTM, RAD, NED + // + + // To: UTM->MAG, RAD, NED + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad + convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg + convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD->DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, RAD->DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + + // + // From: UTM, DEG, ENU + // + + // To: UTM->MAG, DEG, ENU + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + declinationDeg - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + declinationDeg - convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + declinationRad - convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG->RAD, ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG->RAD, ENU->NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // + // From: UTM, DEG, NED + // + + // To: UTM->MAG, DEG, NED + azimuth.unit = Az::UNIT_DEG; azimuth.orientation = Az::ORIENTATION_NED; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = 90; + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - declinationDeg + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - declinationRad + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 - declinationDeg + convergenceDeg), maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->MAG, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 - declinationRad + convergenceRad), maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 + convergenceDeg, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG->RAD, NED + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_NED, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + convergenceRad, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_DEG, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(90 - (90 + convergenceDeg) + 360, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_DEG, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); + + // To: UTM->GEO, DEG->RAD, NED->ENU + maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 - (M_PI_2 + convergenceRad) + 2 * M_PI, maybeAzimuth->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertWithInitialVals) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 10.0; + fix.altitude = 200.0; + + compass_conversions::CompassConverter converter(log, true); + converter.setNavSatPos(fix); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + double declinationDeg = 4.04; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 0.777177; + double convergenceRad = angles::from_degrees(convergenceDeg); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertForcedDeclination) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 10.0; + fix.altitude = 200.0; + + compass_conversions::CompassConverter converter(log, true); + converter.forceMagneticDeclination(angles::from_degrees(5.0)); + converter.setNavSatPos(fix); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + double declinationDeg = 5.0; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 0.777177; + double convergenceRad = angles::from_degrees(convergenceDeg); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertForcedConvergence) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + sensor_msgs::NavSatFix fix; + fix.latitude = 51.0; + fix.longitude = 10.0; + fix.altitude = 200.0; + + compass_conversions::CompassConverter converter(log, true); + converter.forceUTMGridConvergence(angles::from_degrees(5.0)); + converter.setNavSatPos(fix); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + double declinationDeg = 4.04; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 5.0; + double convergenceRad = angles::from_degrees(convergenceDeg); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertForcedBoth) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + converter.forceMagneticDeclination(angles::from_degrees(5.0)); + converter.forceUTMGridConvergence(angles::from_degrees(1.0)); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + + double declinationDeg = 5.0; + double declinationRad = angles::from_degrees(declinationDeg); + double convergenceDeg = 1.0; + double convergenceRad = angles::from_degrees(convergenceDeg); + + azimuth.unit = Az::UNIT_RAD; azimuth.orientation = Az::ORIENTATION_ENU; azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + auto maybeAzimuth = converter.convertAzimuth(azimuth, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(azimuth.header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2 + declinationRad - convergenceRad, maybeAzimuth->azimuth, 1e-2); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(CompassConverter, ConvertQuaternion) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + azimuth.unit = Az::UNIT_RAD; + azimuth.orientation = Az::ORIENTATION_ENU; + azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + + const auto maybeQuat = converter.convertToQuaternion(azimuth); + ASSERT_TRUE(maybeQuat.has_value()); + EXPECT_EQ(azimuth.header, maybeQuat->header); + EXPECT_NEAR(0, maybeQuat->quaternion.x, 1e-4); + EXPECT_NEAR(0, maybeQuat->quaternion.y, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybeQuat->quaternion.z, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybeQuat->quaternion.w, 1e-4); + + auto maybeAzimuth = converter.convertQuaternion( + *maybeQuat, 0.0, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(maybeQuat->header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2, maybeAzimuth->azimuth, 1e-6); + EXPECT_NEAR(0, maybeAzimuth->variance, 1e-6); + EXPECT_EQ(maybeAzimuth->unit, Az::UNIT_RAD); + EXPECT_EQ(maybeAzimuth->orientation, Az::ORIENTATION_ENU); + EXPECT_EQ(maybeAzimuth->reference, Az::REFERENCE_MAGNETIC); + + maybeAzimuth = converter.convertQuaternion( + maybeQuat->quaternion, maybeQuat->header, 0.0, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + ASSERT_TRUE(maybeAzimuth.has_value()); + EXPECT_EQ(maybeQuat->header, maybeAzimuth->header); + EXPECT_NEAR(M_PI_2, maybeAzimuth->azimuth, 1e-6); + EXPECT_NEAR(0, maybeAzimuth->variance, 1e-6); + EXPECT_EQ(maybeAzimuth->unit, Az::UNIT_RAD); + EXPECT_EQ(maybeAzimuth->orientation, Az::ORIENTATION_ENU); + EXPECT_EQ(maybeAzimuth->reference, Az::REFERENCE_MAGNETIC); +} + +TEST(CompassConverter, ConvertToPose) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + azimuth.unit = Az::UNIT_RAD; + azimuth.orientation = Az::ORIENTATION_ENU; + azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + azimuth.variance = 4.0; + + const auto maybePose = converter.convertToPose(azimuth); + ASSERT_TRUE(maybePose.has_value()); + EXPECT_EQ(azimuth.header, maybePose->header); + EXPECT_NEAR(0, maybePose->pose.pose.orientation.x, 1e-4); + EXPECT_NEAR(0, maybePose->pose.pose.orientation.y, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybePose->pose.pose.orientation.z, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybePose->pose.pose.orientation.w, 1e-4); + EXPECT_FALSE(std::isfinite(maybePose->pose.covariance[0 * 6 + 0])); + EXPECT_FALSE(std::isfinite(maybePose->pose.covariance[1 * 6 + 1])); + EXPECT_FALSE(std::isfinite(maybePose->pose.covariance[2 * 6 + 2])); + EXPECT_NE(0.0, maybePose->pose.covariance[3 * 6 + 3]); + EXPECT_NE(0.0, maybePose->pose.covariance[4 * 6 + 4]); + EXPECT_NEAR(4.0, maybePose->pose.covariance[5 * 6 + 5], 1e-4); +} + +TEST(CompassConverter, ConvertToImu) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + compass_conversions::CompassConverter converter(log, true); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + Az azimuth; + azimuth.header.frame_id = "test"; + azimuth.header.stamp = time; + azimuth.unit = Az::UNIT_RAD; + azimuth.orientation = Az::ORIENTATION_ENU; + azimuth.reference = Az::REFERENCE_UTM; + azimuth.azimuth = M_PI_2; + azimuth.variance = 4.0; + + const auto maybeImu = converter.convertToImu(azimuth); + ASSERT_TRUE(maybeImu.has_value()); + EXPECT_EQ(azimuth.header, maybeImu->header); + EXPECT_NEAR(0, maybeImu->orientation.x, 1e-4); + EXPECT_NEAR(0, maybeImu->orientation.y, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybeImu->orientation.z, 1e-4); + EXPECT_NEAR(M_SQRT1_2, maybeImu->orientation.w, 1e-4); + EXPECT_NE(0.0, maybeImu->orientation_covariance[0 * 3 + 0]); + EXPECT_NE(0.0, maybeImu->orientation_covariance[1 * 3 + 1]); + EXPECT_NEAR(4.0, maybeImu->orientation_covariance[2 * 3 + 2], 1e-4); + EXPECT_EQ(-1.0, maybeImu->angular_velocity_covariance[0]); + EXPECT_EQ(-1.0, maybeImu->linear_acceleration_covariance[0]); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/compass_conversions/test/test_compass_transformer_nodelet.cpp b/compass_conversions/test/test_compass_transformer_nodelet.cpp new file mode 100644 index 0000000..70ea529 --- /dev/null +++ b/compass_conversions/test/test_compass_transformer_nodelet.cpp @@ -0,0 +1,1282 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for transformations of compass_msgs. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ros +{ +namespace names +{ +extern void init(const ros::M_string& remappings); +} +} + +using Az = compass_msgs::Azimuth; + +ros::V_string my_argv; + +template +std::unique_ptr createNodelet(const cras::LogHelperPtr& log, + const ros::M_string& remaps = {}, + const std::shared_ptr& tf = nullptr) +{ + // Declaration order of these variables is important to make sure they can be properly stopped and destroyed. + auto nodelet = class_loader::impl::createInstance( + "compass_conversions::CompassTransformerNodelet", nullptr); + if (nodelet == nullptr) + return nullptr; + + { + const auto paramHelper = dynamic_cast(nodelet); + if (paramHelper != nullptr) + paramHelper->setLogger(log); + } + + const auto targetNodelet = dynamic_cast(nodelet); + if (targetNodelet == nullptr) + { + delete nodelet; + return nullptr; + } + + if (tf != nullptr) + targetNodelet->setBuffer(tf); + + nodelet->init(ros::this_node::getName(), remaps, my_argv, nullptr, nullptr); + + return std::unique_ptr(targetNodelet); +} + +TEST(CompassTransformerNodelet, BasicConversion) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, TfConversion) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "deg"); + pnh.setParam("target_orientation", "ned"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_frame", "test2"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + geometry_msgs::TransformStamped tf; + tf.header.stamp = ros::Time::now(); + tf.header.frame_id = "test"; + tf.child_frame_id = "test2"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, tf.transform.rotation); + + auto tfBuffer = std::make_shared(); + tfBuffer->setTransform(tf, "test", true); + + auto nodelet = createNodelet(log, {}, tfBuffer); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0; + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ("test2", lastAz->header.frame_id); + EXPECT_NEAR(180.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_DEG, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_NED, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, TfConversionFail) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "deg"); + pnh.setParam("target_orientation", "ned"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_frame", "test_nonexistent"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + geometry_msgs::TransformStamped tf; + tf.header.stamp = ros::Time::now(); + tf.header.frame_id = "test"; + tf.child_frame_id = "test2"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, tf.transform.rotation); + + auto tfBuffer = std::make_shared(); + tfBuffer->setTransform(tf, "test", true); + + auto nodelet = createNodelet(log, {}, tfBuffer); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0; + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_FALSE(lastAz.has_value()); +} + +TEST(CompassTransformerNodelet, FixMissing) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_reference", "utm"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0; + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_FALSE(lastAz.has_value()); +} + +TEST(CompassTransformerNodelet, FixFromParams) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_reference", "geographic"); + pnh.setParam("initial_lat", 51.0); + pnh.setParam("initial_lon", 15.0); + pnh.setParam("initial_alt", 200.0); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = cras::parseTime("2024-11-18T13:00:00Z"); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(-5.333 + 360, angles::to_degrees(lastAz->azimuth), 1e-3); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, FixFromMsg) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_reference", "geographic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto fixPub = nh.advertise("fix", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && + (azimuthPub.getNumSubscribers() == 0 || fixPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for fix and azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(fixPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + const auto time = cras::parseTime("2024-11-18T13:00:00Z"); + + sensor_msgs::NavSatFix fix; + fix.header.stamp = time; + fix.header.frame_id = "test"; + fix.latitude = 51.0; + fix.longitude = 15.0; + fix.altitude = 200.0; + fixPub.publish(fix); + + // Wait until the fix message is received + for (size_t i = 0; i < 10; ++i) + { + ros::spinOnce(); + ros::WallDuration(0.01).sleep(); + } + + Az in; + in.header.stamp = time; + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(-5.333 + 360, angles::to_degrees(lastAz->azimuth), 1e-3); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubImuNameDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = nh.advertise("imu/data/mag/ned/imu", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + const ros::M_string remaps = { + {pnh.resolveName("azimuth_in"), nh.resolveName("imu/data/mag/ned/imu")}, + }; + + auto nodelet = createNodelet(log, remaps); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + sensor_msgs::Imu in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.orientation); + in.orientation_covariance[2 * 3 + 2] = 4.0; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubImuNoDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("input_orientation", "ned"); + pnh.setParam("input_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + sensor_msgs::Imu in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.orientation); + in.orientation_covariance[2 * 3 + 2] = 4.0; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubPoseNameDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = nh.advertise("pose/mag/ned/pose", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + const ros::M_string remaps = { + {pnh.resolveName("azimuth_in"), nh.resolveName("pose/mag/ned/pose")}, + }; + + auto nodelet = createNodelet(log, remaps); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + geometry_msgs::PoseWithCovarianceStamped in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.pose.pose.orientation); + in.pose.covariance[5 * 6 + 5] = 4; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubPoseNoDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("input_orientation", "ned"); + pnh.setParam("input_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + geometry_msgs::PoseWithCovarianceStamped in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.pose.pose.orientation); + in.pose.covariance[5 * 6 + 5] = 4.0; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubQuatNameDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("input_variance", 4.0); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = nh.advertise("quat/mag/ned/quat", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + const ros::M_string remaps = { + {pnh.resolveName("azimuth_in"), nh.resolveName("quat/mag/ned/quat")}, + }; + + auto nodelet = createNodelet(log, remaps); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + geometry_msgs::QuaternionStamped in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.quaternion); + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, SubQuatNoDetect) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("input_orientation", "ned"); + pnh.setParam("input_reference", "magnetic"); + pnh.setParam("input_variance", 4.0); + + cras::optional lastAz; + auto cb = [&lastAz](const Az::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + geometry_msgs::QuaternionStamped in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.quaternion); + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, lastAz->azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, lastAz->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, lastAz->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, lastAz->reference); + EXPECT_EQ(4.0, lastAz->variance); +} + +TEST(CompassTransformerNodelet, PubImu) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "imu"); + + cras::optional lastAz; + auto cb = [&lastAz](const sensor_msgs::Imu::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->orientation), 1e-6); + EXPECT_EQ(4.0, lastAz->orientation_covariance[2 * 3 + 2]); +} + +TEST(CompassTransformerNodelet, PubImuSuffix) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "imu"); + pnh.setParam("target_append_suffix", true); + + cras::optional lastAz; + auto cb = [&lastAz](const sensor_msgs::Imu::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out/mag/enu/imu", 1, cb); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->orientation), 1e-6); + EXPECT_EQ(4.0, lastAz->orientation_covariance[2 * 3 + 2]); +} + +TEST(CompassTransformerNodelet, PubPose) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "pose"); + + cras::optional lastAz; + auto cb = [&lastAz](const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->pose.pose.orientation), 1e-6); + EXPECT_EQ(4.0, lastAz->pose.covariance[5 * 6 + 5]); +} + +TEST(CompassTransformerNodelet, PubPoseSuffix) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "pose"); + pnh.setParam("target_append_suffix", true); + + cras::optional lastAz; + auto cb = [&lastAz](const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out/mag/enu/pose", 1, cb); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->pose.pose.orientation), 1e-6); + EXPECT_EQ(4.0, lastAz->pose.covariance[5 * 6 + 5]); +} + +TEST(CompassTransformerNodelet, PubQuat) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "quaternion"); + + cras::optional lastAz; + auto cb = [&lastAz](const geometry_msgs::QuaternionStamped::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->quaternion), 1e-6); +} + +TEST(CompassTransformerNodelet, PubQuatSuffix) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "quaternion"); + pnh.setParam("target_append_suffix", true); + + cras::optional lastAz; + auto cb = [&lastAz](const geometry_msgs::QuaternionStamped::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out/mag/enu/quat", 1, cb); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + Az in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + in.azimuth = 90.0; + in.variance = 4.0 * std::pow(180.0 / M_PI, 2.0); + in.unit = Az::UNIT_DEG; + in.orientation = Az::ORIENTATION_NED; + in.reference = Az::REFERENCE_MAGNETIC; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->quaternion), 1e-6); +} + +TEST(CompassTransformerNodelet, CrossType) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("target_unit", "rad"); + pnh.setParam("target_orientation", "enu"); + pnh.setParam("target_reference", "magnetic"); + pnh.setParam("target_type", "quaternion"); + pnh.setParam("input_orientation", "ned"); + pnh.setParam("input_reference", "magnetic"); + + cras::optional lastAz; + auto cb = [&lastAz](const geometry_msgs::QuaternionStamped::ConstPtr& msg) + { + lastAz = *msg; + }; + + auto azimuthPub = pnh.advertise("azimuth_in", 1); + auto azimuthSub = pnh.subscribe("azimuth_out", 1, cb); + + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + auto nodelet = createNodelet(log); + ASSERT_NE(nullptr, nodelet); + + for (size_t i = 0; i < 1000 && (azimuthPub.getNumSubscribers() == 0 || azimuthSub.getNumPublishers() == 0); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for azimuth input and output topics."); + } + + ASSERT_GT(azimuthPub.getNumSubscribers(), 0); + ASSERT_GT(azimuthSub.getNumPublishers(), 0); + + sensor_msgs::Imu in; + in.header.stamp = ros::Time::now(); + in.header.frame_id = "test"; + tf2::Quaternion q; + q.setRPY(0, 0, M_PI_2); + tf2::convert(q, in.orientation); + in.orientation_covariance[2 * 3 + 2] = 4.0; + + azimuthPub.publish(in); + + for (size_t i = 0; i < 10 && !lastAz.has_value() && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastAz.has_value()); + + EXPECT_EQ(in.header.stamp, lastAz->header.stamp); + EXPECT_EQ(in.header.frame_id, lastAz->header.frame_id); + EXPECT_NEAR(0.0, cras::getYaw(lastAz->quaternion), 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + // Remove the program name from argv because the nodelet handling code does not expect it + argc -= 1; + argv += 1; + ros::removeROSArgs(argc, argv, my_argv); + uint32_t initOptions = ros::init_options::AnonymousName; + ros::init(argc, argv, "test_compass_transformer_nodelet", initOptions); + // Anonymous nodes have a problem that topic remappings of type ~a:=b are resolved against the node name without the + // anonymous part. Fix that by running names::init() again after ros::init() finishes and the full node name is known. + // This was reported and a fix provided in https://github.com/ros/ros_comm/issues/2324, but the fix never landed. + ros::names::init(ros::names::getUnresolvedRemappings()); + + ros::NodeHandle nh; // Just prevent ROS being uninited when the test-private nodehandles go out of scope + + return RUN_ALL_TESTS(); +} diff --git a/compass_conversions/test/test_compass_transformer_nodelet.test b/compass_conversions/test/test_compass_transformer_nodelet.test new file mode 100644 index 0000000..197df11 --- /dev/null +++ b/compass_conversions/test/test_compass_transformer_nodelet.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/compass_conversions/test/test_message_filter.cpp b/compass_conversions/test/test_message_filter.cpp new file mode 100644 index 0000000..8a386d0 --- /dev/null +++ b/compass_conversions/test/test_message_filter.cpp @@ -0,0 +1,257 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for compass message filter. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +template +class TestInput : public message_filters::SimpleFilter +{ +public: + void add(const typename T::ConstPtr& msg) + { + // Pass a complete MessageEvent to avoid calling ros::Time::now() to determine the missing timestamp + this->signalMessage(ros::MessageEvent(msg, msg->header.stamp)); + } +}; + +TEST(MessageFilter, NoNavSatNeeded) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + TestInput azimuthInput; + compass_conversions::CompassFilter filter( + log, nullptr, azimuthInput, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + + Az::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + Az::Ptr inMessage(new Az); + inMessage->header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage->unit = Az::UNIT_DEG; + inMessage->orientation = Az::ORIENTATION_NED; + inMessage->reference = Az::REFERENCE_GEOGRAPHIC; + inMessage->azimuth = 90; + + outMessage.reset(); + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(0, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); +} + +TEST(MessageFilter, NavSatNeededButNotGiven) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + TestInput azimuthInput; + TestInput fixInput; + compass_conversions::CompassFilter filter( + log, nullptr, azimuthInput, fixInput, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + + Az::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + Az::Ptr inMessage(new Az); + inMessage->header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage->unit = Az::UNIT_DEG; + inMessage->orientation = Az::ORIENTATION_NED; + inMessage->reference = Az::REFERENCE_MAGNETIC; + inMessage->azimuth = 90; + + outMessage.reset(); + azimuthInput.add(inMessage); + + ASSERT_EQ(nullptr, outMessage); +} + +TEST(MessageFilter, NavSatNeeded) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + TestInput azimuthInput; + TestInput fixInput; + compass_conversions::CompassFilter filter( + log, nullptr, azimuthInput, fixInput, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + + Az::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + Az::Ptr inMessage(new Az); + inMessage->header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage->unit = Az::UNIT_DEG; + inMessage->orientation = Az::ORIENTATION_NED; + inMessage->reference = Az::REFERENCE_MAGNETIC; + inMessage->azimuth = 90; + + outMessage.reset(); + azimuthInput.add(inMessage); + + ASSERT_EQ(nullptr, outMessage); + + sensor_msgs::NavSatFix::Ptr fixMessage(new sensor_msgs::NavSatFix); + fixMessage->header.stamp = inMessage->header.stamp; + fixMessage->latitude = 51; + fixMessage->longitude = 10; + fixMessage->altitude = 200; + fixInput.add(fixMessage); + + ASSERT_EQ(nullptr, outMessage); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); +} + +TEST(MessageFilter, NavSatNeededAndGivenAsInitValue) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + TestInput azimuthInput; + auto converter = std::make_shared(log, true); + compass_conversions::CompassFilter filter( + log, converter, azimuthInput, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + + Az::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + Az::Ptr inMessage(new Az); + inMessage->header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage->unit = Az::UNIT_DEG; + inMessage->orientation = Az::ORIENTATION_NED; + inMessage->reference = Az::REFERENCE_MAGNETIC; + inMessage->azimuth = 90; + + outMessage.reset(); + azimuthInput.add(inMessage); + + ASSERT_EQ(nullptr, outMessage); + + sensor_msgs::NavSatFix::Ptr fixMessage(new sensor_msgs::NavSatFix); + fixMessage->header.stamp = inMessage->header.stamp; + fixMessage->latitude = 51; + fixMessage->longitude = 10; + fixMessage->altitude = 200; + converter->setNavSatPos(*fixMessage); + + ASSERT_EQ(nullptr, outMessage); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); +} + +TEST(MessageFilter, ForcedDeclination) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + TestInput azimuthInput; + auto converter = std::make_shared(log, true); + compass_conversions::CompassFilter filter( + log, converter, azimuthInput, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + + Az::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + Az::Ptr inMessage(new Az); + inMessage->header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage->unit = Az::UNIT_DEG; + inMessage->orientation = Az::ORIENTATION_NED; + inMessage->reference = Az::REFERENCE_MAGNETIC; + inMessage->azimuth = 90; + + outMessage.reset(); + azimuthInput.add(inMessage); + + ASSERT_EQ(nullptr, outMessage); + + converter->forceMagneticDeclination(angles::from_degrees(4.04)); + + ASSERT_EQ(nullptr, outMessage); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); + + azimuthInput.add(inMessage); + + ASSERT_NE(nullptr, outMessage); + EXPECT_NEAR(-angles::from_degrees(4.04) + 2 * M_PI, outMessage->azimuth, 1e-3); + EXPECT_EQ(Az::UNIT_RAD, outMessage->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, outMessage->reference); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/compass_conversions/test/test_tf2_compass_msgs.cpp b/compass_conversions/test/test_tf2_compass_msgs.cpp new file mode 100644 index 0000000..fb534d1 --- /dev/null +++ b/compass_conversions/test/test_tf2_compass_msgs.cpp @@ -0,0 +1,172 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for transformations of compass_msgs. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; + +TEST(TF2CompassMsgs, Tf2MessageTraits) // NOLINT +{ + Az inMessage; + inMessage.header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage.header.frame_id = "test"; + + EXPECT_EQ(inMessage.header.frame_id, tf2::getFrameId(inMessage)); + EXPECT_EQ(inMessage.header.stamp, tf2::getTimestamp(inMessage)); + EXPECT_EQ(inMessage, tf2::toMsg(inMessage)); + + Az msg2; + tf2::fromMsg(inMessage, msg2); + EXPECT_EQ(inMessage, msg2); +} + +TEST(TF2CompassMsgs, TransformRadNed) // NOLINT +{ + Az inMessage; + inMessage.header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage.header.frame_id = "test"; + inMessage.unit = Az::UNIT_RAD; + inMessage.orientation = Az::ORIENTATION_NED; + inMessage.azimuth = M_PI_2; + + geometry_msgs::TransformStamped transform; + transform.header.stamp = inMessage.header.stamp; + transform.header.frame_id = inMessage.header.frame_id; + transform.child_frame_id = "test2"; + // Translation has no effect + transform.transform.translation.x = transform.transform.translation.y = transform.transform.translation.z = 1; + transform.transform.rotation.z = std::sin(M_PI_4 / 2); + transform.transform.rotation.w = std::cos(M_PI_4 / 2); + + Az outMessage; + for (const auto ref : std::list{Az::REFERENCE_MAGNETIC, Az::REFERENCE_GEOGRAPHIC, Az::REFERENCE_UTM}) + { + SCOPED_TRACE(ref); + inMessage.reference = ref; + tf2::doTransform(inMessage, outMessage, transform); + + EXPECT_NEAR(M_PI_4, outMessage.azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, outMessage.unit); + EXPECT_EQ(Az::ORIENTATION_NED, outMessage.orientation); + EXPECT_EQ(ref, outMessage.reference); + } +} + +TEST(TF2CompassMsgs, TransformRadEnu) // NOLINT +{ + Az inMessage; + inMessage.header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage.header.frame_id = "test"; + inMessage.unit = Az::UNIT_RAD; + inMessage.orientation = Az::ORIENTATION_ENU; + inMessage.azimuth = M_PI_2; + + geometry_msgs::TransformStamped transform; + transform.header.stamp = inMessage.header.stamp; + transform.header.frame_id = inMessage.header.frame_id; + transform.child_frame_id = "test2"; + // Translation has no effect + transform.transform.translation.x = transform.transform.translation.y = transform.transform.translation.z = 1; + transform.transform.rotation.z = std::sin(M_PI_4 / 2); + transform.transform.rotation.w = std::cos(M_PI_4 / 2); + + Az outMessage; + for (const auto ref : std::list{Az::REFERENCE_MAGNETIC, Az::REFERENCE_GEOGRAPHIC, Az::REFERENCE_UTM}) + { + SCOPED_TRACE(ref); + inMessage.reference = ref; + tf2::doTransform(inMessage, outMessage, transform); + + EXPECT_NEAR(3 * M_PI_4, outMessage.azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_RAD, outMessage.unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage.orientation); + EXPECT_EQ(ref, outMessage.reference); + } +} + +TEST(TF2CompassMsgs, TransformDegNed) // NOLINT +{ + Az inMessage; + inMessage.header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage.header.frame_id = "test"; + inMessage.unit = Az::UNIT_DEG; + inMessage.orientation = Az::ORIENTATION_NED; + inMessage.azimuth = 90; + + geometry_msgs::TransformStamped transform; + transform.header.stamp = inMessage.header.stamp; + transform.header.frame_id = inMessage.header.frame_id; + transform.child_frame_id = "test2"; + // Translation has no effect + transform.transform.translation.x = transform.transform.translation.y = transform.transform.translation.z = 1; + transform.transform.rotation.z = std::sin(M_PI_4 / 2); + transform.transform.rotation.w = std::cos(M_PI_4 / 2); + + Az outMessage; + for (const auto ref : std::list{Az::REFERENCE_MAGNETIC, Az::REFERENCE_GEOGRAPHIC, Az::REFERENCE_UTM}) + { + SCOPED_TRACE(ref); + inMessage.reference = ref; + tf2::doTransform(inMessage, outMessage, transform); + + EXPECT_NEAR(45, outMessage.azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_DEG, outMessage.unit); + EXPECT_EQ(Az::ORIENTATION_NED, outMessage.orientation); + EXPECT_EQ(ref, outMessage.reference); + } +} + +TEST(TF2CompassMsgs, TransformDegEnu) // NOLINT +{ + Az inMessage; + inMessage.header.stamp = cras::parseTime("2024-11-18T13:00:00.000Z"); + inMessage.header.frame_id = "test"; + inMessage.unit = Az::UNIT_DEG; + inMessage.orientation = Az::ORIENTATION_ENU; + inMessage.azimuth = 90; + + geometry_msgs::TransformStamped transform; + transform.header.stamp = inMessage.header.stamp; + transform.header.frame_id = inMessage.header.frame_id; + transform.child_frame_id = "test2"; + // Translation has no effect + transform.transform.translation.x = transform.transform.translation.y = transform.transform.translation.z = 1; + transform.transform.rotation.z = std::sin(M_PI_4 / 2); + transform.transform.rotation.w = std::cos(M_PI_4 / 2); + + Az outMessage; + for (const auto ref : std::list{Az::REFERENCE_MAGNETIC, Az::REFERENCE_GEOGRAPHIC, Az::REFERENCE_UTM}) + { + SCOPED_TRACE(ref); + inMessage.reference = ref; + tf2::doTransform(inMessage, outMessage, transform); + + EXPECT_NEAR(3 * 45, outMessage.azimuth, 1e-6); + EXPECT_EQ(Az::UNIT_DEG, outMessage.unit); + EXPECT_EQ(Az::ORIENTATION_ENU, outMessage.orientation); + EXPECT_EQ(ref, outMessage.reference); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/compass_conversions/test/test_topic_names.cpp b/compass_conversions/test/test_topic_names.cpp new file mode 100644 index 0000000..1451d0a --- /dev/null +++ b/compass_conversions/test/test_topic_names.cpp @@ -0,0 +1,183 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for naming topics according to the type of Azimuth message they carry. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include + +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; +using Imu = sensor_msgs::Imu; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Quat = geometry_msgs::QuaternionStamped; + +static const auto rad = Az::UNIT_RAD; +static const auto deg = Az::UNIT_DEG; +static const auto enu = Az::ORIENTATION_ENU; +static const auto ned = Az::ORIENTATION_NED; +static const auto mag = Az::REFERENCE_MAGNETIC; +static const auto geo = Az::REFERENCE_GEOGRAPHIC; +static const auto utm = Az::REFERENCE_UTM; + +TEST(TopicNames, GetSuffix) // NOLINT +{ + EXPECT_EQ("mag/enu/rad", compass_conversions::getAzimuthTopicSuffix(rad, enu, mag)); + EXPECT_EQ("mag/enu/deg", compass_conversions::getAzimuthTopicSuffix(deg, enu, mag)); + EXPECT_EQ("mag/enu/imu", compass_conversions::getAzimuthTopicSuffix(rad, enu, mag)); + EXPECT_EQ("mag/enu/pose", compass_conversions::getAzimuthTopicSuffix(rad, enu, mag)); + EXPECT_EQ("mag/enu/quat", compass_conversions::getAzimuthTopicSuffix(rad, enu, mag)); + + EXPECT_EQ("mag/ned/rad", compass_conversions::getAzimuthTopicSuffix(rad, ned, mag)); + EXPECT_EQ("mag/ned/deg", compass_conversions::getAzimuthTopicSuffix(deg, ned, mag)); + EXPECT_EQ("mag/ned/imu", compass_conversions::getAzimuthTopicSuffix(rad, ned, mag)); + EXPECT_EQ("mag/ned/pose", compass_conversions::getAzimuthTopicSuffix(rad, ned, mag)); + EXPECT_EQ("mag/ned/quat", compass_conversions::getAzimuthTopicSuffix(rad, ned, mag)); + + EXPECT_EQ("true/enu/rad", compass_conversions::getAzimuthTopicSuffix(rad, enu, geo)); + EXPECT_EQ("true/enu/deg", compass_conversions::getAzimuthTopicSuffix(deg, enu, geo)); + EXPECT_EQ("true/enu/imu", compass_conversions::getAzimuthTopicSuffix(rad, enu, geo)); + EXPECT_EQ("true/enu/pose", compass_conversions::getAzimuthTopicSuffix(rad, enu, geo)); + EXPECT_EQ("true/enu/quat", compass_conversions::getAzimuthTopicSuffix(rad, enu, geo)); + + EXPECT_EQ("true/ned/rad", compass_conversions::getAzimuthTopicSuffix(rad, ned, geo)); + EXPECT_EQ("true/ned/deg", compass_conversions::getAzimuthTopicSuffix(deg, ned, geo)); + EXPECT_EQ("true/ned/imu", compass_conversions::getAzimuthTopicSuffix(rad, ned, geo)); + EXPECT_EQ("true/ned/pose", compass_conversions::getAzimuthTopicSuffix(rad, ned, geo)); + EXPECT_EQ("true/ned/quat", compass_conversions::getAzimuthTopicSuffix(rad, ned, geo)); + + EXPECT_EQ("utm/enu/rad", compass_conversions::getAzimuthTopicSuffix(rad, enu, utm)); + EXPECT_EQ("utm/enu/deg", compass_conversions::getAzimuthTopicSuffix(deg, enu, utm)); + EXPECT_EQ("utm/enu/imu", compass_conversions::getAzimuthTopicSuffix(rad, enu, utm)); + EXPECT_EQ("utm/enu/pose", compass_conversions::getAzimuthTopicSuffix(rad, enu, utm)); + EXPECT_EQ("utm/enu/quat", compass_conversions::getAzimuthTopicSuffix(rad, enu, utm)); + + EXPECT_EQ("utm/ned/rad", compass_conversions::getAzimuthTopicSuffix(rad, ned, utm)); + EXPECT_EQ("utm/ned/deg", compass_conversions::getAzimuthTopicSuffix(deg, ned, utm)); + EXPECT_EQ("utm/ned/imu", compass_conversions::getAzimuthTopicSuffix(rad, ned, utm)); + EXPECT_EQ("utm/ned/pose", compass_conversions::getAzimuthTopicSuffix(rad, ned, utm)); + EXPECT_EQ("utm/ned/quat", compass_conversions::getAzimuthTopicSuffix(rad, ned, utm)); +} + +TEST(TopicNames, ParseTopicOnlySuffix) // NOLINT +{ + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("mag/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("mag/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("mag/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("mag/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, mag), compass_conversions::parseAzimuthTopicName("mag/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("mag/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("mag/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("mag/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("mag/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, mag), compass_conversions::parseAzimuthTopicName("mag/ned/deg")); + + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("true/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("true/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("true/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("true/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, geo), compass_conversions::parseAzimuthTopicName("true/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("true/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("true/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("true/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("true/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, geo), compass_conversions::parseAzimuthTopicName("true/ned/deg")); + + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("utm/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("utm/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("utm/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("utm/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, utm), compass_conversions::parseAzimuthTopicName("utm/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("utm/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("utm/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("utm/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("utm/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, utm), compass_conversions::parseAzimuthTopicName("utm/ned/deg")); +} + +TEST(TopicNames, ParseTopicNotOnlySuffix) // NOLINT +{ + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, mag), compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/deg")); + + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, geo), compass_conversions::parseAzimuthTopicName("azimuth/test/true/ned/deg")); + + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/enu/rad")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/enu/imu")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/enu/pose")); + EXPECT_EQ(std::make_tuple(rad, enu, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/enu/quat")); + EXPECT_EQ(std::make_tuple(deg, enu, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/enu/deg")); + + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/ned/rad")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/ned/imu")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/ned/pose")); + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/ned/quat")); + EXPECT_EQ(std::make_tuple(deg, ned, utm), compass_conversions::parseAzimuthTopicName("azimuth/test/utm/ned/deg")); +} + +TEST(TopicNames, ParseTopicWrong) // NOLINT +{ + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("azimuth").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("azimuth/test").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("foo").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("rad/enu/mag").has_value()); + + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("foo/ned/deg").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("mag/foo/deg").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("mag/ned/foo").has_value()); + + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("azimuth/test/foo/ned/deg").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("azimuth/test/mag/foo/deg").has_value()); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName("azimuth/test/mag/ned/foo").has_value()); +} + +TEST(TopicNames, ParseTopicFromConnectionHeader) // NOLINT +{ + boost::shared_ptr header; + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName(header).has_value()); + + header.reset(new ros::M_string{}); + EXPECT_FALSE(compass_conversions::parseAzimuthTopicName(header).has_value()); + + (*header)["topic"] = "azimuth/test/utm/ned/rad"; + EXPECT_EQ(std::make_tuple(rad, ned, utm), compass_conversions::parseAzimuthTopicName(header)); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/compass_msgs/CHANGELOG.rst b/compass_msgs/CHANGELOG.rst index 427e96e..88d1a4f 100644 --- a/compass_msgs/CHANGELOG.rst +++ b/compass_msgs/CHANGELOG.rst @@ -1,3 +1,6 @@ +.. SPDX-License-Identifier: BSD-3-Clause +.. SPDX-FileCopyrightText: Czech Technical University in Prague + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package compass_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/compass_msgs/CMakeLists.txt b/compass_msgs/CMakeLists.txt index e1b72da..5d91b3f 100644 --- a/compass_msgs/CMakeLists.txt +++ b/compass_msgs/CMakeLists.txt @@ -1,7 +1,10 @@ -cmake_minimum_required(VERSION 3.0.2) +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +cmake_minimum_required(VERSION 3.10.2) project(compass_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) +find_package(catkin REQUIRED COMPONENTS cras_cpp_common message_generation std_msgs) add_message_files( FILES @@ -13,4 +16,47 @@ generate_messages( std_msgs ) -catkin_package(CATKIN_DEPENDS message_runtime std_msgs) +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_string_utils + CATKIN_DEPENDS message_runtime std_msgs +) + +include_directories(include ${${PROJECT_NAME}_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME}_string_utils src/string_utils.cpp) +add_dependencies(${PROJECT_NAME}_string_utils ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_string_utils ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_string_utils + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + find_package(rostest REQUIRED) + + roslint_custom(catkin_lint "-W2" .) + + # Roslint C++ - checks formatting and some other rules for C++ files + + file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) + file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) + file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) + + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-build/include,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright") + roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC} ${ROSLINT_TEST}) + + roslint_add_test() + + catkin_add_gtest(test_string_utils test/test_string_utils.cpp) + target_link_libraries(test_string_utils ${PROJECT_NAME}_string_utils) +endif() \ No newline at end of file diff --git a/compass_msgs/README.md b/compass_msgs/README.md index bc5ec18..60b1b71 100644 --- a/compass_msgs/README.md +++ b/compass_msgs/README.md @@ -1,3 +1,6 @@ + + + # compass\_msgs Messages for use with a compass. \ No newline at end of file diff --git a/compass_msgs/include/compass_msgs/string_utils.h b/compass_msgs/include/compass_msgs/string_utils.h new file mode 100644 index 0000000..d070a56 --- /dev/null +++ b/compass_msgs/include/compass_msgs/string_utils.h @@ -0,0 +1,67 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief String utilities for compass_msgs. + * \author Martin Pecka + */ + +#include + +#include + +namespace compass_msgs +{ + +/** + * \brief Convert the given azimuth unit to string. + * \param[in] unit The unit. + * \return The string. + * \throw std::runtime_error If the unit is invalid. + */ +std::string unitToString(decltype(Azimuth::unit) unit); + +/** + * \brief Convert the given string to azimuth unit. + * \param[in] unitStr A string describing the unit. + * \return The unit. + * \throw std::runtime_error If the unit is invalid. + */ +decltype(Azimuth::unit) parseUnit(const std::string& unitStr); + +/** + * \brief Convert the given azimuth orientation to string. + * \param[in] orientation The orientation. + * \return The string. + * \throw std::runtime_error If the orientation is invalid. + */ +std::string orientationToString(decltype(Azimuth::orientation) orientation); + +/** + * \brief Convert the given string to azimuth orientation. + * \param[in] orientationStr A string describing the orientation. + * \return The orientation. + * \throw std::runtime_error If the orientation is invalid. + */ +decltype(Azimuth::orientation) parseOrientation(const std::string& orientationStr); + +/** + * \brief Convert the given azimuth reference to string. + * \param[in] reference The reference. + * \return The string. + * \throw std::runtime_error If the reference is invalid. + */ +std::string referenceToString(decltype(Azimuth::reference) reference); + +/** + * \brief Convert the given string to azimuth reference. + * \param[in] referenceStr A string describing the reference. + * \return The reference. + * \throw std::runtime_error If the reference is invalid. + */ +decltype(Azimuth::reference) parseReference(const std::string& referenceStr); + +} diff --git a/compass_msgs/msg/Azimuth.msg b/compass_msgs/msg/Azimuth.msg index 43f0703..46f3c8e 100644 --- a/compass_msgs/msg/Azimuth.msg +++ b/compass_msgs/msg/Azimuth.msg @@ -1,9 +1,13 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + Header header -# The represented azimuth angle (angular difference between a given direction and north/east) +# The represented azimuth angle (angular difference between a given direction and north/east) in unit corresponding to +# the 'unit' field. float64 azimuth -# Variance of the measurement (i.e. standard deviation squared) +# Variance of the measurement (i.e. standard deviation squared) in units corresponding to squared 'unit' field. float64 variance # Unit of measurement: diff --git a/compass_msgs/package.xml b/compass_msgs/package.xml index 65237e3..646d255 100644 --- a/compass_msgs/package.xml +++ b/compass_msgs/package.xml @@ -1,5 +1,7 @@ - + + + compass_msgs 1.0.3 Messages related to compass @@ -7,16 +9,30 @@ Martin Pecka Martin Pecka - https://github.com/ctu-vras/gps-navigation + https://github.com/ctu-vras/compass + https://github.com/ctu-vras/compass/issues + https://wiki.ros.org/compass_msgs - BSD + BSD catkin std_msgs + cras_cpp_common message_generation message_runtime + + cras_cpp_common message_runtime + + python-catkin-lint + python3-catkin-lint + roslint + rostest + + + + diff --git a/compass_msgs/rosdoc.yaml b/compass_msgs/rosdoc.yaml new file mode 100644 index 0000000..8211934 --- /dev/null +++ b/compass_msgs/rosdoc.yaml @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +- builder: doxygen + required: True + file_patterns: '*.h *.hpp *.hh *.dox /README.md' + use_mdfile_as_mainpage: 'README.md' + generate_treeview: True diff --git a/compass_msgs/src/string_utils.cpp b/compass_msgs/src/string_utils.cpp new file mode 100644 index 0000000..2012447 --- /dev/null +++ b/compass_msgs/src/string_utils.cpp @@ -0,0 +1,95 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief String utilities for compass_msgs. + * \author Martin Pecka + */ + +#include + +#include +#include +#include + +namespace compass_msgs +{ + +std::string unitToString(const decltype(Azimuth::unit) unit) +{ + switch (unit) + { + case Azimuth::UNIT_RAD: + return "rad"; + case Azimuth::UNIT_DEG: + return "deg"; + default: + throw std::runtime_error(cras::format("Invalid Azimuth::unit %u", unit)); + } +} + +decltype(Azimuth::unit) parseUnit(const std::string& unitStr) +{ + const auto unit = cras::toLower(unitStr); + if (unit == "rad") + return Azimuth::UNIT_RAD; + else if (unit == "deg") + return Azimuth::UNIT_DEG; + else + throw std::runtime_error(cras::format("Invalid Azimuth::unit '%s'", unitStr.c_str())); +} + +std::string orientationToString(const decltype(Azimuth::orientation) orientation) +{ + switch (orientation) + { + case Azimuth::ORIENTATION_ENU: + return "ENU"; + case Azimuth::ORIENTATION_NED: + return "NED"; + default: + throw std::runtime_error(cras::format("Invalid Azimuth::orientation %u", orientation)); + } +} + +decltype(Azimuth::orientation) parseOrientation(const std::string& orientationStr) +{ + const auto orientation = cras::toLower(orientationStr); + if (orientation == "enu") + return Azimuth::ORIENTATION_ENU; + else if (orientation == "ned") + return Azimuth::ORIENTATION_NED; + else + throw std::runtime_error(cras::format("Invalid Azimuth::orientation '%s'", orientationStr.c_str())); +} + +std::string referenceToString(const decltype(Azimuth::reference) reference) +{ + switch (reference) + { + case Azimuth::REFERENCE_MAGNETIC: + return "magnetic"; + case Azimuth::REFERENCE_GEOGRAPHIC: + return "geographic"; + case Azimuth::REFERENCE_UTM: + return "UTM"; + default: + throw std::runtime_error(cras::format("Invalid Azimuth::reference %u", reference)); + } +} + +decltype(Azimuth::reference) parseReference(const std::string& referenceStr) +{ + const auto reference = cras::toLower(referenceStr); + if (reference == "magnetic") + return Azimuth::REFERENCE_MAGNETIC; + else if (reference == "geographic" || reference == "true") + return Azimuth::REFERENCE_GEOGRAPHIC; + else if (reference == "utm") + return Azimuth::REFERENCE_UTM; + else + throw std::runtime_error(cras::format("Invalid Azimuth::reference '%s'", referenceStr.c_str())); +} + +} diff --git a/compass_msgs/test/test_string_utils.cpp b/compass_msgs/test/test_string_utils.cpp new file mode 100644 index 0000000..acf9457 --- /dev/null +++ b/compass_msgs/test/test_string_utils.cpp @@ -0,0 +1,81 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for testing string utilities for compass_msgs. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include + +#include + +using Az = compass_msgs::Azimuth; + +TEST(CompassMsgs, Unit) // NOLINT +{ + EXPECT_EQ(Az::UNIT_RAD, compass_msgs::parseUnit("rad")); + EXPECT_EQ(Az::UNIT_RAD, compass_msgs::parseUnit("RAD")); + EXPECT_EQ(Az::UNIT_RAD, compass_msgs::parseUnit("Rad")); + + EXPECT_EQ(Az::UNIT_DEG, compass_msgs::parseUnit("deg")); + EXPECT_EQ(Az::UNIT_DEG, compass_msgs::parseUnit("DEG")); + EXPECT_EQ(Az::UNIT_DEG, compass_msgs::parseUnit("Deg")); + + EXPECT_THROW(compass_msgs::parseUnit("foo"), std::runtime_error); + + EXPECT_EQ("rad", compass_msgs::unitToString(Az::UNIT_RAD)); + EXPECT_EQ("deg", compass_msgs::unitToString(Az::UNIT_DEG)); + EXPECT_THROW(compass_msgs::unitToString(10), std::runtime_error); +} + +TEST(CompassMsgs, Orientation) // NOLINT +{ + EXPECT_EQ(Az::ORIENTATION_ENU, compass_msgs::parseOrientation("enu")); + EXPECT_EQ(Az::ORIENTATION_ENU, compass_msgs::parseOrientation("ENU")); + EXPECT_EQ(Az::ORIENTATION_ENU, compass_msgs::parseOrientation("Enu")); + + EXPECT_EQ(Az::ORIENTATION_NED, compass_msgs::parseOrientation("ned")); + EXPECT_EQ(Az::ORIENTATION_NED, compass_msgs::parseOrientation("NED")); + EXPECT_EQ(Az::ORIENTATION_NED, compass_msgs::parseOrientation("Ned")); + + EXPECT_THROW(compass_msgs::parseOrientation("foo"), std::runtime_error); + + EXPECT_EQ("ENU", compass_msgs::orientationToString(Az::ORIENTATION_ENU)); + EXPECT_EQ("NED", compass_msgs::orientationToString(Az::ORIENTATION_NED)); + EXPECT_THROW(compass_msgs::orientationToString(10), std::runtime_error); +} + +TEST(CompassMsgs, Reference) // NOLINT +{ + EXPECT_EQ(Az::REFERENCE_MAGNETIC, compass_msgs::parseReference("magnetic")); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, compass_msgs::parseReference("MAGNETIC")); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, compass_msgs::parseReference("Magnetic")); + + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("geographic")); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("GEOGRAPHIC")); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("Geographic")); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("true")); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("TRUE")); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, compass_msgs::parseReference("True")); + + EXPECT_EQ(Az::REFERENCE_UTM, compass_msgs::parseReference("utm")); + EXPECT_EQ(Az::REFERENCE_UTM, compass_msgs::parseReference("UTM")); + EXPECT_EQ(Az::REFERENCE_UTM, compass_msgs::parseReference("Utm")); + + EXPECT_THROW(compass_msgs::parseReference("foo"), std::runtime_error); + + EXPECT_EQ("magnetic", compass_msgs::referenceToString(Az::REFERENCE_MAGNETIC)); + EXPECT_EQ("geographic", compass_msgs::referenceToString(Az::REFERENCE_GEOGRAPHIC)); + EXPECT_EQ("UTM", compass_msgs::referenceToString(Az::REFERENCE_UTM)); + EXPECT_THROW(compass_msgs::referenceToString(10), std::runtime_error); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/magnetic_model/CMakeLists.txt b/magnetic_model/CMakeLists.txt new file mode 100644 index 0000000..08a124a --- /dev/null +++ b/magnetic_model/CMakeLists.txt @@ -0,0 +1,77 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +cmake_minimum_required(VERSION 3.10.2) +project(magnetic_model) + +set(CMAKE_CXX_STANDARD 17) + +find_package(catkin REQUIRED COMPONENTS + angles + compass_msgs + cras_cpp_common + geometry_msgs + roscpp + roslib + sensor_msgs +) + +# Ubuntu libgeographic-dev package installs into non-standard location +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS compass_msgs cras_cpp_common geometry_msgs roscpp sensor_msgs +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} src/${PROJECT_NAME}.cpp src/${PROJECT_NAME}_manager.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} PRIVATE ${GeographicLib_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY data/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data +) + +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + + roslint_custom(catkin_lint "-W2" .) + + # Roslint C++ - checks formatting and some other rules for C++ files + + file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) + file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) + file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) + + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-build/include,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright") + roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC} ${ROSLINT_TEST}) + + roslint_add_test() + + catkin_add_gtest(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_compile_definitions(test_${PROJECT_NAME} PRIVATE -DTEST_DATA_DIR="${CMAKE_SOURCE_DIR}/data/magnetic") +endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/magnetic_model/README.md b/magnetic_model/README.md new file mode 100644 index 0000000..a1f75e7 --- /dev/null +++ b/magnetic_model/README.md @@ -0,0 +1,17 @@ + + + +# magnetic\_model + +This package contains utilities for working with the +[World Magnetic Model](https://en.wikipedia.org/wiki/World_Magnetic_Model), e.g. computing magnetic declination. + +## C++ Libraries + +### magnetic\_model::MagneticModel + +A class that can examine the Earth magnetic model and answer questions about it like field strength and components. + +### magnetic\_model::MagneticModelManager + +A manager class for `MagneticModel`s that can automatically load and return the most suitable model. \ No newline at end of file diff --git a/magnetometer_compass/data/magnetic/README.md b/magnetic_model/data/magnetic/README.md similarity index 74% rename from magnetometer_compass/data/magnetic/README.md rename to magnetic_model/data/magnetic/README.md index 6c03afa..8a090ab 100644 --- a/magnetometer_compass/data/magnetic/README.md +++ b/magnetic_model/data/magnetic/README.md @@ -1,3 +1,6 @@ + + + This folder contains WMM Earth magnetic field models. Newer models can be downloaded either directly from https://downloads.sourceforge.net/project/geographiclib/magnetic-distrib/.tar.bz2?use_mirror=autoselect, or using tool `geographiclib-get-magnetic` from package `geographiclib-tools`. The validity of the WMM models is usually 5 years. \ No newline at end of file diff --git a/magnetometer_compass/data/magnetic/wmm2010.wmm b/magnetic_model/data/magnetic/wmm2010.wmm similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2010.wmm rename to magnetic_model/data/magnetic/wmm2010.wmm diff --git a/magnetometer_compass/data/magnetic/wmm2010.wmm.cof b/magnetic_model/data/magnetic/wmm2010.wmm.cof similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2010.wmm.cof rename to magnetic_model/data/magnetic/wmm2010.wmm.cof diff --git a/magnetic_model/data/magnetic/wmm2010.wmm.cof.license b/magnetic_model/data/magnetic/wmm2010.wmm.cof.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2010.wmm.cof.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetic_model/data/magnetic/wmm2010.wmm.license b/magnetic_model/data/magnetic/wmm2010.wmm.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2010.wmm.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetometer_compass/data/magnetic/wmm2015v2.wmm b/magnetic_model/data/magnetic/wmm2015v2.wmm similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2015v2.wmm rename to magnetic_model/data/magnetic/wmm2015v2.wmm diff --git a/magnetometer_compass/data/magnetic/wmm2015v2.wmm.cof b/magnetic_model/data/magnetic/wmm2015v2.wmm.cof similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2015v2.wmm.cof rename to magnetic_model/data/magnetic/wmm2015v2.wmm.cof diff --git a/magnetic_model/data/magnetic/wmm2015v2.wmm.cof.license b/magnetic_model/data/magnetic/wmm2015v2.wmm.cof.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2015v2.wmm.cof.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetic_model/data/magnetic/wmm2015v2.wmm.license b/magnetic_model/data/magnetic/wmm2015v2.wmm.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2015v2.wmm.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetometer_compass/data/magnetic/wmm2020.wmm b/magnetic_model/data/magnetic/wmm2020.wmm similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2020.wmm rename to magnetic_model/data/magnetic/wmm2020.wmm diff --git a/magnetometer_compass/data/magnetic/wmm2020.wmm.cof b/magnetic_model/data/magnetic/wmm2020.wmm.cof similarity index 100% rename from magnetometer_compass/data/magnetic/wmm2020.wmm.cof rename to magnetic_model/data/magnetic/wmm2020.wmm.cof diff --git a/magnetic_model/data/magnetic/wmm2020.wmm.cof.license b/magnetic_model/data/magnetic/wmm2020.wmm.cof.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2020.wmm.cof.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetic_model/data/magnetic/wmm2020.wmm.license b/magnetic_model/data/magnetic/wmm2020.wmm.license new file mode 100644 index 0000000..f159e42 --- /dev/null +++ b/magnetic_model/data/magnetic/wmm2020.wmm.license @@ -0,0 +1,2 @@ +SPDX-License-Identifier: LicenseRef-WMM-Public-Domain +SPDX-FileCopyrightText: Copyright NOAA \ No newline at end of file diff --git a/magnetic_model/include/magnetic_model/magnetic_model.h b/magnetic_model/include/magnetic_model/magnetic_model.h new file mode 100644 index 0000000..ae607f1 --- /dev/null +++ b/magnetic_model/include/magnetic_model/magnetic_model.h @@ -0,0 +1,128 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Earth magnetic field model. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace magnetic_model +{ + +/** + * \brief Properties of a local magnetic field vector. + */ +struct MagneticField +{ + sensor_msgs::MagneticField field; //!< The computed magnetic field vector in ENU convention [T]. + geometry_msgs::Vector3 dt; //!< Time derivative of the field [T/year]. + geometry_msgs::Vector3 error; //!< Standard deviations of the magnetic field vector components. +}; + +/** + * \brief Components into which a 3D magnetic vector can be decomposed. + */ +struct MagneticFieldComponents +{ + double horizontalMagnitude; //!< Magnitude of the horizontal component [T]. + double totalMagnitude; //!< Total magnitude of the vector [T]. + double declination; //!< Declination of the vector (angle from North towards East) [rad]. + double inclination; //!< Inclination of the vector (angle from horizontal direction downwards) [rad]. +}; + +/** + * \brief Magnetic field components together with their time derivatives and error terms. + */ +struct MagneticFieldComponentProperties +{ + MagneticFieldComponents values; //!< Values of the components [T or rad]. + MagneticFieldComponents dt; //!< Time derivatives of the components [T/year or rad/year]. + MagneticFieldComponents errors; //!< Standard deviations of the components [T or rad]. +}; + +struct MagneticModelPrivate; + +/** + * \brief Earth magnetic model. + */ +class MagneticModel : public cras::HasLogger +{ +public: + static const char* WMM2010; //!< WMM 2010 model name + static const char* WMM2015; //!< WMM 2015v2 model name + static const char* WMM2020; //!< WMM 2020 model name + + /** + * \brief Create the magnetic model. + * + * \param[in] log The logger. + * \param[in] name Name of the model (e.g. "wmm2020"). + * \param[in] modelPath Path to the folder with stored models. If empty, a default system location will be used. + * \param[in] strict Whether to fail if the magnetic model is used outside its natural validity bounds. + */ + MagneticModel(const cras::LogHelperPtr& log, const std::string& name, const std::string& modelPath, bool strict); + virtual ~MagneticModel(); + + /** + * \brief Tell whether this model is valid at the given time instant. + * \param[in] time The time of validity. + * \return Whether the model is valid (regardless of strictness). + */ + virtual bool isValid(const ros::Time& time) const; + + /** + * \brief Tell whether this model is valid at the given year. + * \param[in] year The year of validity. + * \return Whether the model is valid (regardless of strictness). + */ + virtual bool isValid(int year) const; + + /** + * \brief Get the magnetic field vector on the provided place on Earth. + * \param[in] fix The place for which magnetic field is queried. Timestamp from the message is ignored. + * \param[in] stamp The time for which magnetic field is queried. + * \return The magnetic field. + */ + virtual cras::expected getMagneticField( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const; + + /** + * \brief Get the magnetic field components on the provided place on Earth. + * \param[in] fix The place for which magnetic field components are queried. Timestamp from the message is ignored. + * \param[in] stamp The time for which magnetic field components are queried. + * \return The magnetic field components. + */ + virtual cras::expected getMagneticFieldComponents( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const; + + /** + * \brief Get the magnetic field components on the provided place on Earth. + * \param[in] fix The place for which magnetic field components are queried. Timestamp from the message is ignored. + * \param[in] stamp The time for which magnetic field components are queried. + * \return The magnetic field components. + */ + virtual cras::expected getMagneticFieldComponents( + const MagneticField& field, const ros::Time& stamp) const; + +protected: + //! \brief If true, the magnetic model will fail if it is used outside its bounds. + bool strict {true}; + + //! \brief PIMPL data + std::unique_ptr data; +}; + +} diff --git a/magnetic_model/include/magnetic_model/magnetic_model_manager.h b/magnetic_model/include/magnetic_model/magnetic_model_manager.h new file mode 100644 index 0000000..091c93e --- /dev/null +++ b/magnetic_model/include/magnetic_model/magnetic_model_manager.h @@ -0,0 +1,89 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Manager of magnetic field models. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include + +namespace magnetic_model +{ + +struct MagneticModelManagerPrivate; + +/** + * \brief Earth magnetic model. + */ +class MagneticModelManager : public cras::HasLogger +{ +public: + /** + * \brief Create magnetic model manager. + * + * \param[in] log The logger. + * \param[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`. + */ + explicit MagneticModelManager(const cras::LogHelperPtr& log, const cras::optional& modelPath = {}); + virtual ~MagneticModelManager(); + + /** + * \brief Get the model path. + * \return Path to the folder with stored models. If empty, a default system location will be used. + */ + std::string getModelPath() const; + + /** + * \brief Set the path to the folder with stored models. + * \param[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`. + */ + void setModelPath(const cras::optional& modelPath); + + /** + * \brief Get the best magnetic model for the given date. + * \param[in] date The date in question. + * \return Name of the best magnetic model. If forcedMagneticModelName is non-empty, this value is always returned. + */ + virtual std::string getBestMagneticModelName(const ros::Time& date) const; + + /** + * \brief Get the most suitable magnetic model for the given year. + * \param[in] stamp The time for which model should be returned. + * \param[in] strict Whether the returned model should fail if data outside its validity range are queried. + * \return The magnetic model or error if there is no suitable model. + */ + virtual cras::expected, std::string> getMagneticModel( + const ros::Time& stamp, bool strict) const; + + /** + * \brief Get the magnetic model with the given name. + * \param[in] name Name of the model. Check MagneticModel constants to see possible values. + * \param[in] strict Whether the returned model should fail if data outside its validity range are queried. + * \return The magnetic model or error if it cannot be found. + */ + virtual cras::expected, std::string> getMagneticModel( + const std::string& name, bool strict) const; + +protected: + //! \brief PIMPL data + std::unique_ptr data; +}; + +} diff --git a/magnetic_model/package.xml b/magnetic_model/package.xml new file mode 100644 index 0000000..4133bd0 --- /dev/null +++ b/magnetic_model/package.xml @@ -0,0 +1,42 @@ + + + + + magnetic_model + 1.0.3 + World Magnetic Model ROS API. + + Martin Pecka + Martin Pecka + + BSD + Public Domain + + https://github.com/ctu-vras/compass + https://github.com/ctu-vras/compass/issues + https://wiki.ros.org/magnetic_model + + catkin + + angles + geographiclib + roslib + + angles + geographiclib + roslib + + compass_msgs + cras_cpp_common + geometry_msgs + roscpp + sensor_msgs + + python-catkin-lint + python3-catkin-lint + roslint + + + + + diff --git a/magnetic_model/rosdoc.yaml b/magnetic_model/rosdoc.yaml new file mode 100644 index 0000000..8211934 --- /dev/null +++ b/magnetic_model/rosdoc.yaml @@ -0,0 +1,8 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +- builder: doxygen + required: True + file_patterns: '*.h *.hpp *.hh *.dox /README.md' + use_mdfile_as_mainpage: 'README.md' + generate_treeview: True diff --git a/magnetic_model/src/magnetic_model.cpp b/magnetic_model/src/magnetic_model.cpp new file mode 100644 index 0000000..719d292 --- /dev/null +++ b/magnetic_model/src/magnetic_model.cpp @@ -0,0 +1,297 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Earth magnetic field model. + * \author Martin Pecka + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetic_model +{ + +const char* MagneticModel::WMM2010 = "wmm2010"; +const char* MagneticModel::WMM2015 = "wmm2015v2"; +const char* MagneticModel::WMM2020 = "wmm2020"; + +using Az = compass_msgs::Azimuth; + +struct ModelErrors +{ + double X; // nT + double Y; // nT + double Z; // nT + double H; // nT + double F; // nT + double D_ofs; + double D_lin; + double I; // degrees +}; + +struct MagneticModelPrivate +{ + //! \brief The GeographicLib magnetic field model. + std::unique_ptr magneticModel; + + ModelErrors errors{}; +}; + +MagneticModel::MagneticModel( + const cras::LogHelperPtr& log, const std::string& name, const std::string& modelPath, const bool strict) : + cras::HasLogger(log), strict(strict), data(new MagneticModelPrivate{}) +{ + try + { + this->data->magneticModel = std::make_unique(name, modelPath); + } + catch (const GeographicLib::GeographicErr& e) + { + throw std::invalid_argument(cras::format( + "Could not create magnetic field model %s because of the following error: %s", name.c_str(), e.what())); + } + + if (name == WMM2010) + { + // https://geomag.bgs.ac.uk/documents/WMM2010_Report.pdf, Section 3.3 + this->data->errors.X = 405; + this->data->errors.Y = 244; + this->data->errors.Z = 559; + this->data->errors.H = 371; + this->data->errors.F = 614; + this->data->errors.I = 0.38; + this->data->errors.D_ofs = 1.71; + this->data->errors.D_lin = 0; + } + else if (name == WMM2015) + { + // https://repository.library.noaa.gov/view/noaa/48055/noaa_48055_DS1.pdf?download-document-submit=Download, Sec 3.4 + this->data->errors.X = 138; + this->data->errors.Y = 89; + this->data->errors.Z = 165; + this->data->errors.H = 133; + this->data->errors.F = 152; + this->data->errors.I = 0.22; + this->data->errors.D_ofs = 0.23; + this->data->errors.D_lin = 5430; + } + else if (name == WMM2020) + { + // https://repository.library.noaa.gov/view/noaa/24390/noaa_24390_DS1.pdf?download-document-submit=Download, Sec 3.4 + this->data->errors.X = 131; + this->data->errors.Y = 94; + this->data->errors.Z = 157; + this->data->errors.H = 128; + this->data->errors.F = 145; + this->data->errors.I = 0.21; + this->data->errors.D_ofs = 0.26; + this->data->errors.D_lin = 5625; + } + + CRAS_INFO("Initialized magnetic model %s.", name.c_str()); +} + +MagneticModel::~MagneticModel() = default; +bool MagneticModel::isValid(const ros::Time& time) const +{ + return this->isValid(cras::getYear(time)); +} + +bool MagneticModel::isValid(const int year) const +{ + return year >= this->data->magneticModel->MinTime() && year < this->data->magneticModel->MaxTime(); +} + +cras::expected MagneticModel::getMagneticField( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const +{ + double errorCoef = 1.0; + + const auto year = cras::getYear(stamp); + const auto minYear = this->data->magneticModel->MinTime(); + const auto maxYear = this->data->magneticModel->MaxTime(); + if (year < minYear || year > maxYear) + { + const auto err = cras::format("Using magnetic field model %s for an invalid year %u!", + this->data->magneticModel->MagneticModelName().c_str(), year); + if (this->strict) + { + return cras::make_unexpected(err); + } + else + { + CRAS_ERROR_THROTTLE(10.0, "%s", err.c_str()); + errorCoef *= std::max(1.0, std::max(std::abs(year - minYear), std::abs(year - maxYear))); + } + } + + const auto minAlt = this->data->magneticModel->MinHeight(); + const auto maxAlt = this->data->magneticModel->MaxHeight(); + if (fix.altitude < minAlt || fix.altitude > maxAlt) + { + const auto err = cras::format( + "Using magnetic field model %s in altitude %.01f m which is outside the model range.", + this->data->magneticModel->MagneticModelName().c_str(), fix.altitude); + if (this->strict) + { + return cras::make_unexpected(err); + } + else + { + CRAS_ERROR_THROTTLE(10.0, "%s", err.c_str()); + errorCoef *= std::max(1.0, std::max(std::abs(fix.altitude - minAlt), std::abs(fix.altitude - maxAlt)) / 1000.0); + } + } + + MagneticField result; + (*this->data->magneticModel)(year, fix.latitude, fix.longitude, fix.altitude, + result.field.magnetic_field.x, result.field.magnetic_field.y, result.field.magnetic_field.z, + result.dt.x, result.dt.y, result.dt.z); + + result.field.header.frame_id = fix.header.frame_id; + result.field.header.stamp = stamp; + result.field.magnetic_field.x *= 1e-9; + result.field.magnetic_field.y *= 1e-9; + result.field.magnetic_field.z *= 1e-9; + result.dt.x *= 1e-9; + result.dt.y *= 1e-9; + result.dt.z *= 1e-9; + result.error.x = errorCoef * this->data->errors.X * 1e-9; + result.error.y = errorCoef * this->data->errors.Y * 1e-9; + result.error.z = errorCoef * this->data->errors.Z * 1e-9; + result.field.magnetic_field_covariance[0 * 3 + 0] = std::pow(result.error.x, 2); + result.field.magnetic_field_covariance[1 * 3 + 1] = std::pow(result.error.y, 2); + result.field.magnetic_field_covariance[2 * 3 + 2] = std::pow(result.error.z, 2); + + return result; +} + +cras::expected MagneticModel::getMagneticFieldComponents( + const sensor_msgs::NavSatFix& fix, const ros::Time& stamp) const +{ + double errorCoef = 1.0; + const auto minAlt = this->data->magneticModel->MinHeight(); + const auto maxAlt = this->data->magneticModel->MaxHeight(); + if (fix.altitude < minAlt || fix.altitude > maxAlt) + { + const auto err = cras::format( + "Using magnetic field model %s in altitude %.01f m which is outside the model range.", + this->data->magneticModel->MagneticModelName().c_str(), fix.altitude); + if (this->strict) + { + return cras::make_unexpected(err); + } + else + { + CRAS_ERROR_THROTTLE(10.0, "%s", err.c_str()); + errorCoef *= std::max(1.0, std::max(std::abs(fix.altitude - minAlt), std::abs(fix.altitude - maxAlt)) / 1000.0); + } + } + + const auto field = this->getMagneticField(fix, stamp); + if (!field.has_value()) + return cras::make_unexpected(field.error()); + + auto result = this->getMagneticFieldComponents(*field, stamp); + if (result.has_value()) + { + result->errors.horizontalMagnitude *= errorCoef; + result->errors.totalMagnitude *= errorCoef; + result->errors.declination *= errorCoef; + result->errors.inclination *= errorCoef; + } + + return result; +} + +cras::expected MagneticModel::getMagneticFieldComponents( + const MagneticField& field, const ros::Time& stamp) const +{ + double errorCoef = 1.0; + const auto year = cras::getYear(stamp); + const auto minYear = this->data->magneticModel->MinTime(); + const auto maxYear = this->data->magneticModel->MaxTime(); + if (year < minYear || year > maxYear) + { + const auto err = cras::format("Using magnetic field model %s for an invalid year %u!", + this->data->magneticModel->MagneticModelName().c_str(), year); + if (this->strict) + { + return cras::make_unexpected(err); + } + else + { + CRAS_ERROR_THROTTLE(10.0, "%s", err.c_str()); + errorCoef *= std::max(1.0, std::max(std::abs(year - minYear), std::abs(year - maxYear))); + } + } + + MagneticFieldComponentProperties res; + GeographicLib::MagneticModel::FieldComponents( + field.field.magnetic_field.x * 1e9, field.field.magnetic_field.y * 1e9, field.field.magnetic_field.z * 1e9, + field.dt.x * 1e9, field.dt.y * 1e9, field.dt.z * 1e9, + res.values.horizontalMagnitude, res.values.totalMagnitude, res.values.declination, res.values.inclination, + res.dt.horizontalMagnitude, res.dt.totalMagnitude, res.dt.declination, res.dt.inclination); + + // Compute declination before scaling down the results from nT to T + res.errors.declination = errorCoef * angles::from_degrees(std::sqrt( + std::pow(this->data->errors.D_ofs, 2) + + std::pow(this->data->errors.D_lin / res.values.horizontalMagnitude, 2))); + res.values.horizontalMagnitude *= 1e-9; + res.values.totalMagnitude *= 1e-9; + res.values.declination = angles::from_degrees(res.values.declination); + res.values.inclination = angles::from_degrees(res.values.inclination); + res.dt.horizontalMagnitude *= 1e-9; + res.dt.totalMagnitude *= 1e-9; + res.dt.declination = angles::from_degrees(res.dt.declination); + res.dt.inclination = angles::from_degrees(res.dt.inclination); + res.errors.horizontalMagnitude = errorCoef * this->data->errors.H * 1e-9; + res.errors.totalMagnitude = errorCoef * this->data->errors.F * 1e-9; + res.errors.inclination = errorCoef * angles::from_degrees(this->data->errors.I); + + tm t{}; + t.tm_year = year - 1900; + t.tm_mon = 0; + t.tm_mday = 1; + t.tm_hour = 0; + t.tm_min = 0; + t.tm_sec = 0; + const auto yearStart = cras::fromStructTm(t); + + t.tm_year = year + 1 - 1900; + const auto nextYearStart = cras::fromStructTm(t); + + double yearFrac {0.0}; + if (yearStart.has_value() && nextYearStart.has_value()) + { + const auto yearDuration = *nextYearStart - *yearStart; + const auto nowSinceYearStart = stamp - *yearStart; + yearFrac = (nowSinceYearStart / yearDuration).toSec(); + } + + res.values.horizontalMagnitude += yearFrac * res.dt.horizontalMagnitude; + res.values.totalMagnitude += yearFrac * res.dt.totalMagnitude; + res.values.declination += yearFrac * res.dt.declination; + res.values.inclination += yearFrac * res.dt.inclination; + + return res; +} + +} diff --git a/magnetic_model/src/magnetic_model_manager.cpp b/magnetic_model/src/magnetic_model_manager.cpp new file mode 100644 index 0000000..9b8150d --- /dev/null +++ b/magnetic_model/src/magnetic_model_manager.cpp @@ -0,0 +1,127 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Earth magnetic field model. + * \author Martin Pecka + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetic_model +{ + +/** + * \brief Private data of MagneticModelManager. + */ +struct MagneticModelManagerPrivate +{ + //! \brief Cache of already initialized magnetic field models. Keys are model names/strictness. + std::map, std::shared_ptr> magneticModels; + + //! \brief Path to the models on disk. Empty means system default. + std::string modelPath; +}; + +MagneticModelManager::MagneticModelManager(const cras::LogHelperPtr& log, const cras::optional& modelPath): + cras::HasLogger(log), data(new MagneticModelManagerPrivate{}) +{ + this->setModelPath(modelPath); +} + +MagneticModelManager::~MagneticModelManager() = default; + +std::string MagneticModelManager::getModelPath() const +{ + return this->data->modelPath; +} + +void MagneticModelManager::setModelPath(const cras::optional& modelPath) +{ + if (modelPath.has_value()) + { + if (modelPath->empty()) + this->data->modelPath = GeographicLib::MagneticModel::DefaultMagneticPath(); + else + this->data->modelPath = *modelPath; + } + else + { + const auto packagePath = ros::package::getPath("magnetic_model"); + if (!packagePath.empty()) + { + this->data->modelPath = packagePath + "/data/magnetic"; + } + else + { + CRAS_ERROR("Could not resolve package magnetic_model. Is the workspace properly sourced?"); + this->data->modelPath = GeographicLib::MagneticModel::DefaultMagneticPath(); + } + } + + this->data->magneticModels.clear(); + + CRAS_INFO("Using WMM models from directory %s.", this->data->modelPath.c_str()); +} + +std::string MagneticModelManager::getBestMagneticModelName(const ros::Time& date) const +{ + const auto year = cras::getYear(date); // If the conversion failed, year would be 0, thus triggering the last branch. + if (year >= 2020) + return MagneticModel::WMM2020; + else if (year >= 2015) + return MagneticModel::WMM2015; + else + return MagneticModel::WMM2010; +} + +cras::expected, std::string> MagneticModelManager::getMagneticModel( + const ros::Time& stamp, const bool strict) const +{ + const auto name = this->getBestMagneticModelName(stamp); + const auto model = this->getMagneticModel(name, strict); + if (!model.has_value()) + return cras::make_unexpected(model.error()); + if (strict && !model.value()->isValid(stamp)) + return cras::make_unexpected(cras::format( + "The best magnetic model %s is not valid at time %s.", name.c_str(), cras::to_pretty_string(stamp).c_str())); + return *model; +} + +cras::expected, std::string> MagneticModelManager::getMagneticModel( + const std::string& name, const bool strict) const +{ + const auto key = std::make_pair(name, strict); + if (this->data->magneticModels.find(key) == this->data->magneticModels.end()) + { + try + { + this->data->magneticModels[key] = std::make_shared(this->log, name, this->data->modelPath, strict); + } + catch (const std::invalid_argument& e) + { + return cras::make_unexpected(e.what()); + } + } + + return this->data->magneticModels[key]; +} + +} diff --git a/magnetic_model/test/test_magnetic_model.cpp b/magnetic_model/test/test_magnetic_model.cpp new file mode 100644 index 0000000..f255d73 --- /dev/null +++ b/magnetic_model/test/test_magnetic_model.cpp @@ -0,0 +1,281 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for magnetic model. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +TEST(MagneticModel, Construct) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + ASSERT_NO_THROW(magnetic_model::MagneticModel model( + log, magnetic_model::MagneticModel::WMM2010, TEST_DATA_DIR, true)); + + ASSERT_NO_THROW(magnetic_model::MagneticModel model( + log, magnetic_model::MagneticModel::WMM2015, TEST_DATA_DIR, true)); + + ASSERT_NO_THROW(magnetic_model::MagneticModel model( + log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true)); + + ASSERT_THROW(magnetic_model::MagneticModel model( + log, "nonexisting", TEST_DATA_DIR, true), std::invalid_argument); +} + +TEST(MagneticModel, GetField) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true); + + sensor_msgs::NavSatFix fix; + fix.header.frame_id = "test"; + fix.header.stamp = cras::parseTime("2024-11-18T13:00:00Z"); + fix.latitude = 51; + fix.longitude = 10; + fix.altitude = 200; + const auto result = model.getMagneticField(fix, fix.header.stamp); + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(fix.header, result->field.header); + EXPECT_NEAR(1.33e-06, result->field.magnetic_field.x, 1e-6); + EXPECT_NEAR(1.95e-05, result->field.magnetic_field.y, 1e-6); + EXPECT_NEAR(-4.54e-05, result->field.magnetic_field.z, 1e-6); + EXPECT_NEAR(1.7161e-14, result->field.magnetic_field_covariance[0 * 3 + 0], 1e-18); + EXPECT_NEAR(8.8360e-15, result->field.magnetic_field_covariance[1 * 3 + 1], 1e-18); + EXPECT_NEAR(2.4649e-14, result->field.magnetic_field_covariance[2 * 3 + 2], 1e-18); + EXPECT_NEAR(5.946503e-08, result->dt.x, 1e-12); + EXPECT_NEAR(2.142373e-10, result->dt.y, 1e-12); + EXPECT_NEAR(-5.489079e-08, result->dt.z, 1e-12); + EXPECT_NEAR(1.31e-07, result->error.x, 1e-12); + EXPECT_NEAR(9.40e-08, result->error.y, 1e-12); + EXPECT_NEAR(1.57e-07, result->error.z, 1e-12); +} + +TEST(MagneticModel, GetFieldWrongYearStrict) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true); + + sensor_msgs::NavSatFix fix; + fix.header.frame_id = "test"; + fix.header.stamp = cras::parseTime("2014-11-18T13:00:00Z"); + + EXPECT_FALSE(model.getMagneticField(fix, fix.header.stamp).has_value()); + EXPECT_FALSE(model.getMagneticFieldComponents(fix, fix.header.stamp).has_value()); +} + +TEST(MagneticModel, GetFieldWrongYearNonStrict) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, false); + + sensor_msgs::NavSatFix fix; + fix.header.frame_id = "test"; + fix.header.stamp = cras::parseTime("2014-11-18T13:00:00Z"); + + EXPECT_TRUE(model.getMagneticField(fix, fix.header.stamp).has_value()); + EXPECT_TRUE(model.getMagneticFieldComponents(fix, fix.header.stamp).has_value()); +} + +TEST(MagneticModel, GetFieldWrongAltitude) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true); + + sensor_msgs::NavSatFix fix; + fix.header.frame_id = "test"; + fix.header.stamp = cras::parseTime("2014-11-18T13:00:00Z"); + fix.altitude = 1e6; + + EXPECT_FALSE(model.getMagneticField(fix, fix.header.stamp).has_value()); + EXPECT_FALSE(model.getMagneticFieldComponents(fix, fix.header.stamp).has_value()); +} + +TEST(MagneticModel, GetFieldComponentsFromField) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true); + + const auto stamp = cras::parseTime("2024-11-18T13:00:00Z"); + + magnetic_model::MagneticField field; + field.field.header.frame_id = "test"; + field.field.header.stamp = stamp; + field.field.magnetic_field.x = 1.33e-06; + field.field.magnetic_field.y = 1.95e-05; + field.field.magnetic_field.z = -4.54e-05; + field.dt.x = 5.946503e-08; + field.dt.y = 2.142373e-10; + field.dt.z = -5.489079e-08; + + const auto result = model.getMagneticFieldComponents(field, stamp); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(1.95e-05, result->values.horizontalMagnitude, 1e-6); + EXPECT_NEAR(4.95e-05, result->values.totalMagnitude, 1e-6); + EXPECT_NEAR(angles::from_degrees(4.04), result->values.declination, 1e-3); + EXPECT_NEAR(1.165, result->values.inclination, 1e-3); + EXPECT_NEAR(4.2601598e-09, result->dt.horizontalMagnitude, 1e-12); + EXPECT_NEAR(5.210166e-08, result->dt.totalMagnitude, 1e-12); + EXPECT_NEAR(3.0346225e-03, result->dt.declination, 1e-6); + EXPECT_NEAR(3.600e-04, result->dt.inclination, 1e-6); + EXPECT_NEAR(1.28e-07, result->errors.horizontalMagnitude, 1e-9); + EXPECT_NEAR(1.45e-07, result->errors.totalMagnitude, 1e-9); + EXPECT_NEAR(0.007, result->errors.declination, 1e-3); + EXPECT_NEAR(0.004, result->errors.inclination, 1e-3); + + EXPECT_FALSE(model.getMagneticFieldComponents(field, cras::parseTime("2014-11-18T13:00:00Z")).has_value()); +} + +TEST(MagneticModel, GetFieldComponentsFromFix) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModel model(log, magnetic_model::MagneticModel::WMM2020, TEST_DATA_DIR, true); + + sensor_msgs::NavSatFix fix; + fix.header.frame_id = "test"; + fix.header.stamp = cras::parseTime("2024-11-18T13:00:00Z"); + fix.latitude = 51; + fix.longitude = 10; + fix.altitude = 200; + + const auto result = model.getMagneticFieldComponents(fix, fix.header.stamp); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(1.95e-05, result->values.horizontalMagnitude, 1e-6); + EXPECT_NEAR(4.95e-05, result->values.totalMagnitude, 1e-6); + EXPECT_NEAR(angles::from_degrees(4.04), result->values.declination, 1e-2); + EXPECT_NEAR(1.165, result->values.inclination, 1e-2); + EXPECT_NEAR(4.2601598e-09, result->dt.horizontalMagnitude, 1e-10); + EXPECT_NEAR(5.210166e-08, result->dt.totalMagnitude, 1e-10); + EXPECT_NEAR(3.0346225e-03, result->dt.declination, 1e-5); + EXPECT_NEAR(3.600e-04, result->dt.inclination, 1e-5); + EXPECT_NEAR(1.28e-07, result->errors.horizontalMagnitude, 1e-9); + EXPECT_NEAR(1.45e-07, result->errors.totalMagnitude, 1e-9); + EXPECT_NEAR(0.007, result->errors.declination, 1e-3); + EXPECT_NEAR(0.004, result->errors.inclination, 1e-3); + + // Use with wrong year + EXPECT_FALSE(model.getMagneticFieldComponents(fix, cras::parseTime("2014-11-18T13:00:00Z")).has_value()); + // Use with wrong altitude + fix.altitude = 1e6; + EXPECT_FALSE(model.getMagneticFieldComponents(fix, fix.header.stamp).has_value()); +} + +TEST(MagneticModelManager, GetBestModelName) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModelManager manager(log, TEST_DATA_DIR); + + EXPECT_EQ( + magnetic_model::MagneticModel::WMM2020, + manager.getBestMagneticModelName(cras::parseTime("2024-11-18T13:00:00Z"))); + + EXPECT_EQ( + magnetic_model::MagneticModel::WMM2015, + manager.getBestMagneticModelName(cras::parseTime("2019-11-18T13:00:00Z"))); + + EXPECT_EQ( + magnetic_model::MagneticModel::WMM2010, + manager.getBestMagneticModelName(cras::parseTime("2014-11-18T13:00:00Z"))); + + EXPECT_EQ( + magnetic_model::MagneticModel::WMM2010, + manager.getBestMagneticModelName(cras::parseTime("2004-11-18T13:00:00Z"))); + + EXPECT_EQ( + magnetic_model::MagneticModel::WMM2020, + manager.getBestMagneticModelName(cras::parseTime("2034-11-18T13:00:00Z"))); +} + +TEST(MagneticModelManager, GetModelByName) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModelManager manager(log, TEST_DATA_DIR); + + ASSERT_TRUE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true).has_value()); + ASSERT_TRUE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, false).has_value()); + EXPECT_NE( + *manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true), + *manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, false)); + + const auto model = *manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true); + EXPECT_TRUE(model->isValid(2024)); + EXPECT_TRUE(model->isValid(2020)); + EXPECT_FALSE(model->isValid(2025)); +} + +TEST(MagneticModelManager, SetModelPath) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModelManager manager(log, TEST_DATA_DIR); + + EXPECT_TRUE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true).has_value()); + EXPECT_EQ(TEST_DATA_DIR, manager.getModelPath()); + + manager.setModelPath("/nonexistent/path"); + EXPECT_FALSE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true).has_value()); + EXPECT_EQ("/nonexistent/path", manager.getModelPath()); + + // Use the default location from GeographicLib. We don't know if the user has downloaded the model or not into the + // default location, so we cannot presume validity of the model. Instead, we just check that the default path gets + // resolved to a nonempty path. + manager.setModelPath(""); + EXPECT_FALSE(manager.getModelPath().empty()); + EXPECT_NE("/nonexistent/path", manager.getModelPath()); + + const auto oldPath = getenv("GEOGRAPHICLIB_MAGNETIC_PATH"); + setenv("GEOGRAPHICLIB_MAGNETIC_PATH", TEST_DATA_DIR, true); + manager.setModelPath(""); + EXPECT_TRUE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true).has_value()); + EXPECT_EQ(TEST_DATA_DIR, manager.getModelPath()); + if (oldPath == nullptr) + unsetenv("GEOGRAPHICLIB_MAGNETIC_PATH"); + else + setenv("GEOGRAPHICLIB_MAGNETIC_PATH", oldPath, true); + + manager.setModelPath(cras::nullopt); + EXPECT_TRUE(manager.getMagneticModel(magnetic_model::MagneticModel::WMM2020, true).has_value()); + EXPECT_FALSE(manager.getModelPath().empty()); +} + +TEST(MagneticModelManager, GetModelByTime) +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + magnetic_model::MagneticModelManager manager(log, TEST_DATA_DIR); + + ASSERT_TRUE(manager.getMagneticModel(cras::parseTime("2024-11-18T13:00:00Z"), true).has_value()); + ASSERT_TRUE(manager.getMagneticModel(cras::parseTime("2014-11-18T13:00:00Z"), true).has_value()); + EXPECT_NE( + *manager.getMagneticModel(cras::parseTime("2024-11-18T13:00:00Z"), true), + *manager.getMagneticModel(cras::parseTime("2014-11-18T13:00:00Z"), true)); + + auto model = *manager.getMagneticModel(cras::parseTime("2024-11-18T13:00:00Z"), true); + EXPECT_TRUE(model->isValid(2024)); + EXPECT_TRUE(model->isValid(2020)); + EXPECT_FALSE(model->isValid(2025)); + + EXPECT_FALSE(manager.getMagneticModel(cras::parseTime("2004-11-18T13:00:00Z"), true).has_value()); + EXPECT_TRUE(manager.getMagneticModel(cras::parseTime("2004-11-18T13:00:00Z"), false).has_value()); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_compass/CHANGELOG.rst b/magnetometer_compass/CHANGELOG.rst index 2472769..fa4db7b 100644 --- a/magnetometer_compass/CHANGELOG.rst +++ b/magnetometer_compass/CHANGELOG.rst @@ -1,3 +1,6 @@ +.. SPDX-License-Identifier: BSD-3-Clause +.. SPDX-FileCopyrightText: Czech Technical University in Prague + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package magnetometer_compass ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/magnetometer_compass/CMakeLists.txt b/magnetometer_compass/CMakeLists.txt index 2b2dd9e..2312fd5 100644 --- a/magnetometer_compass/CMakeLists.txt +++ b/magnetometer_compass/CMakeLists.txt @@ -1,52 +1,99 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + cmake_minimum_required(VERSION 3.10.2) project(magnetometer_compass) +set(CMAKE_CXX_STANDARD 17) + find_package(catkin REQUIRED COMPONENTS angles + compass_conversions compass_msgs cras_cpp_common geometry_msgs imu_transformer + magnetometer_pipeline message_filters nodelet roscpp - roslib sensor_msgs tf2 tf2_geometry_msgs tf2_ros ) -# Ubuntu libgeographic-dev package installs into non-standard location -set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") -find_package(GeographicLib REQUIRED) - -catkin_package() +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS compass_msgs cras_cpp_common sensor_msgs tf2 +) include_directories( include ${catkin_INCLUDE_DIRS} - ${GeographicLib_INCLUDE_DIRS} ) -add_library(magnetometer_compass_nodelet src/magnetometer_compass_nodelet.cpp) -add_dependencies(magnetometer_compass_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(magnetometer_compass_nodelet ${catkin_LIBRARIES} ${GeographicLib_LIBRARIES}) -cras_node_from_nodelet(magnetometer_compass_nodelet "${PROJECT_NAME}/magnetometer_compass_nodelet.h" ${PROJECT_NAME}::MagnetometerCompassNodelet OUTPUT_NAME magnetometer_compass) +add_library(${PROJECT_NAME} src/${PROJECT_NAME}.cpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -install(TARGETS magnetometer_compass_nodelet +add_library(${PROJECT_NAME}_nodelet nodelets/${PROJECT_NAME}_nodelet.cpp) +add_dependencies(${PROJECT_NAME}_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_nodelet ${PROJECT_NAME} ${catkin_LIBRARIES}) +cras_node_from_nodelet(${PROJECT_NAME}_nodelet ${PROJECT_NAME}::MagnetometerCompassNodelet OUTPUT_NAME ${PROJECT_NAME}) + +add_library(visualize_azimuth_nodelet nodelets/visualize_azimuth_nodelet.cpp) +add_dependencies(visualize_azimuth_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(visualize_azimuth_nodelet ${catkin_LIBRARIES}) +cras_node_from_nodelet(visualize_azimuth_nodelet ${PROJECT_NAME}::VisualizeAzimuthNodelet OUTPUT_NAME visualize_azimuth) + +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_nodelet visualize_azimuth_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) -install(DIRECTORY data/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install(FILES nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -catkin_install_python(PROGRAMS nodes/visualize_azimuth - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + find_package(rostest REQUIRED) + find_package(roslaunch REQUIRED) + + roslint_custom(catkin_lint "-W2" .) + + # Roslint C++ - checks formatting and some other rules for C++ files + file(GLOB_RECURSE ROSLINT_FILES include/*.h src/*.cpp nodelets/*.cpp test/*.cpp) + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-build/include,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright") + roslint_cpp(${ROSLINT_FILES}) + + roslint_add_test() + + roslaunch_add_file_check(launch/compass.launch) + + catkin_add_gtest(test_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + + add_rostest_gtest(test_${PROJECT_NAME}_nodelet test/test_${PROJECT_NAME}_nodelet.test test/test_${PROJECT_NAME}_nodelet.cpp) + target_link_libraries(test_${PROJECT_NAME}_nodelet ${catkin_LIBRARIES} ${PROJECT_NAME}_nodelet) + roslaunch_add_file_check(test/test_${PROJECT_NAME}_nodelet.test USE_TEST_DEPENDENCIES) + if(CMAKE_VERSION VERSION_LESS "3.13.0") + set_property(TARGET test_${PROJECT_NAME}_nodelet APPEND_STRING PROPERTY LINK_FLAGS " -Wl,--no-as-needed") + else() + #catkin_lint: ignore_once cmake_old + target_link_options(test_${PROJECT_NAME}_nodelet PUBLIC "LINKER:--no-as-needed") + endif() + + add_rostest_gtest(test_visualize_azimuth_nodelet test/test_visualize_azimuth_nodelet.test test/test_visualize_azimuth_nodelet.cpp) + target_link_libraries(test_visualize_azimuth_nodelet ${catkin_LIBRARIES}) + roslaunch_add_file_check(test/test_visualize_azimuth_nodelet.test USE_TEST_DEPENDENCIES) +endif() diff --git a/magnetometer_compass/LICENSE b/magnetometer_compass/LICENSE deleted file mode 100644 index d6e861a..0000000 --- a/magnetometer_compass/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2022, Martin Pecka -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/magnetometer_compass/README.md b/magnetometer_compass/README.md index 0ac4a06..e9c48af 100644 --- a/magnetometer_compass/README.md +++ b/magnetometer_compass/README.md @@ -1,3 +1,6 @@ + + + # magnetometer\_compass Compass based on a 3-axis magnetometer, attitude readings and possibly also GPS. @@ -42,11 +45,14 @@ may be required to re-estimate the bias from time to time even during runtime. contents of `orientation` and at least roll and pitch should be estimated as well as possible (relative to the gravity vector). These messages should come at the same rate as the magnetometer data (or faster). -- `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer measurements (bias not removed). +- `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer raw measurements (bias not removed) (disabled by param + `~subscribe_mag_unbiased`). - `imu/mag_bias` (`sensor_msgs/MagneticField`): Bias of the magnetometer. This value will be subtracted from the - incoming magnetometer measurements (only the `magnetic_field` field - is relevant). Messages on this topic do not need to come repeatedly - if the bias does not change. + incoming magnetometer measurements. Messages on this topic do not + need to come repeatedly if the bias does not change. Disabled by + param `~subscribe_mag_unbiased`. +- `imu/mag_unbiased` (`sensor_msgs/MagneticField`): 3-axis magnetometer unbiased measurements (enabled by param + `~subscribe_mag_unbiased`). - `gps/fix` (`sensor_msgs/NavSatFix`, optional): GPS fix messages from which the latitude, longitude, altitude and current year can be read. These are further used to compute magnetic declination and UTM grid convergence factor if requested. @@ -124,7 +130,9 @@ may be required to re-estimate the bias from time to time even during runtime. UTM heading in ENU as a pose (translation will always be zero). ### Parameters -- all the `publish_*` parameters mentioned above. +- All the `publish_*` parameters mentioned above. +- Please note that you cannot combine both `~subscribe_mag_unbiased` and `~publish_mag_unbiased` set to true. + Such configuration is invalid and the node will not start. - `~frame` (string, default `base_link`): Frame into which the IMU and magnetometer data should be transformed. - `~low_pass_ratio` (double, default 0.95): The azimuth is filtered with a low-pass filter. This sets its aggressivity (0 means raw measurements, 1 means no updates). @@ -141,21 +149,64 @@ may be required to re-estimate the bias from time to time even during runtime. ignored and this value for declination is forced. This can be useful either if you know the value in advance or in simulation. -- `~magnetic_models_path` (string, defaults to the pre-installed directory): Directory with WMM magnetic field - models. You usually do not need to use other than the preinstalled models. But if you do, specify the path to - the custom models directory here. +- `~utm_grid_convergence` (double, no default, optional, radians): If set, forces this value of UTM grid convergence. +- `~magnetic_models_path` (string, defaults to a pre-installed directory): 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. You usually do not + need to use other than the preinstalled models. But if you do, specify the path to the custom models directory + here. - `~magnetic_model` (string, defaults to autodetection by year): Name of the magnetic field model to use. If omitted, an automated decision is made based on the current year (or `~initial_year`, if set). This model is used for computing magnetic declination. +- `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. -## Node visualize\_azimuth +## Nodelet visualize\_azimuth\_nodelet and node visualize\_azimuth This node visualizes `Azimuth` messages in RViz converting them to a pose. ### Subscribed topics -- `~azimuth` (`compass_msgs/Azimuth`): The azimuth to visualize. +- `~azimuth` (multiple types supported): The azimuth to visualize. Any type supported by compass_conversions package + can be used: `compass_msgs/Azimuth`, `geometry_msgs/Quaternion`, + `geometry_msgs/PoseWithCovarianceStamped` or `sensor_msgs/Imu`. If other + types than `compass_msgs/Azimuth` are used, either the resolved topic name + must contain the azimuth type identification (e.g. end with `mag/enu/imu`), + or you must provide parameters `~input_reference` and `~input_orientation`. +- `gps/fix` (`sensor_msgs/NavSatFix`, optional): GPS fix messages from which the latitude, longitude, altitude and + current year can be read. These are further used to compute + magnetic declination and UTM grid convergence factor. +- `utm_zone` (`std_msgs/Int32`, optional): Optional UTM zone updates. ### Published topics -- `~azimuth_vis` (`geometry_msgs/PoseWithCovarianceStamped`): The pose which visualizes the azimuth. +- `~azimuth_vis` (`sensor_msgs/MagneticField`, enabled by param `~publish_mag_unbiased`, off by default): + The magnetic field measurement with bias removed. + +### Parameters +- `max_rate` (double, optional): If specified, visualization messages frequency will be at most this value \[Hz\]. +- `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. +- `~input_orientation` (str, 'enu' or 'ned', default: unspecified): ENU or NED orientation to be used to interpret + input messages (in case orientation cannot be + derived either from message contents or topic + name). +- `~input_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used to + interpret input messages (in case + reference cannot be derived either + from message contents or topic + name). +- `~input_variance` (double, optional, rad^2): If specified, this variance will be used in the output messages + if variance cannot be determined from the input messages (e.g. for + `QuaternionStamped`). diff --git a/magnetometer_compass/include/magnetometer_compass/magnetometer_compass.h b/magnetometer_compass/include/magnetometer_compass/magnetometer_compass.h new file mode 100644 index 0000000..46132d5 --- /dev/null +++ b/magnetometer_compass/include/magnetometer_compass/magnetometer_compass.h @@ -0,0 +1,100 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Convert magnetometer and IMU measurements to azimuth. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_compass +{ + +struct MagnetometerCompassPrivate; + +/** + * \brief Convert magnetometer and IMU measurements to azimuth. + */ +class MagnetometerCompass : public cras::HasLogger +{ +public: + /** + * \brief Create the compass. + * \param[in] log Logger. + * \param[in] frame The target frame in which the azimuth is expressed. Its Z axis should approx. point upwards. + * Azimuth is the angle between magnetic North and this frame's X axis. + * \param[in] tf TF buffer for transforming incoming data to `frame`. If you are sure data are already in the target + * frame, you can pass an empty buffer. + */ + MagnetometerCompass(const cras::LogHelperPtr& log, const std::string& frame, + const std::shared_ptr& tf); + + /** + * \brief Create the compass. + * \param[in] log Logger. + * \param[in] frame The target frame in which the azimuth is expressed. Its Z axis should approx. point upwards. + * Azimuth is the angle between magnetic North and this frame's X axis. + * \param[in] tf TF buffer for transforming incoming data to `frame`. If you are sure data are already in the target + * frame, you can pass an empty buffer. + */ + MagnetometerCompass(const cras::LogHelperPtr& log, const std::string& frame, + const std::shared_ptr& tf); + + virtual ~MagnetometerCompass(); + + /** + * \brief Configure the bias remover from ROS parameters. + * \param[in] params The parameters. + * + * The following parameters are read: + * - `~initial_variance` (double, default 0): Variance of the measurement used at startup (in rad^2). + * - `~low_pass_ratio` (double, default 0.95): The azimuth is filtered with a low-pass filter. This sets its + * aggressivity (0 means raw measurements, 1 means no updates). + */ + virtual void configFromParams(const cras::BoundParamHelper& params); + + /** + * \brief The azimuth is filtered with a low-pass filter. This sets its aggressivity. + * \param[in] ratio The ratio (0 means raw measurements, 1 means no updates). + */ + virtual void setLowPassRatio(double ratio); + + /** + * \brief Compute azimuth from the provided IMU and magnetometer measurements. + * \param[in] imu IMU tied to the magnetometer. It has to contain valid orientation. + * \param[in] magUnbiased Magnetometer measurement with removed bias. + * \return The computed azimuth, or an error message. The azimuth will be in radians and NED frame. + * \note The function does not check time synchronization of the two inputs. + * \note Both inputs have to be transformable to the configured target frame. + */ + virtual cras::expected computeAzimuth( + const sensor_msgs::Imu& imu, const sensor_msgs::MagneticField& magUnbiased); + + /** + * \brief Reset the computation (i.e. the low-pass filter and estimated variance). + */ + virtual void reset(); + +protected: + /** + * \brief Re-estimate variance after adding a new measurement. + * \note This is not yet implemented. + */ + virtual void updateVariance(); + +private: + std::unique_ptr data; //!< PIMPL +}; +} diff --git a/magnetometer_compass/include/magnetometer_compass/magnetometer_compass_nodelet.h b/magnetometer_compass/include/magnetometer_compass/magnetometer_compass_nodelet.h deleted file mode 100644 index 13cff88..0000000 --- a/magnetometer_compass/include/magnetometer_compass/magnetometer_compass_nodelet.h +++ /dev/null @@ -1,279 +0,0 @@ -#pragma once - -/** - * \file - * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. - * \author Martin Pecka - * SPDX-License-Identifier: BSD-3-Clause - * SPDX-FileCopyrightText: Czech Technical University in Prague - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace magnetometer_compass -{ - -struct MagnetometerCompassNodeletPrivate; - -/** - * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. - * - * Because there is no well established Azimuth message in ROS, this node publishes our custom compass_msgs/Azimuth - * as well as a few other formats that are capable of carrying orientation information. It also offers the azimuth - * values in both radians and degrees, because radians are the ROS standard, while degrees are more used in the - * geographic area. There are tens of possible combinations of the output data formats, so each of the published - * topics has a boolean parameter that enables it. By default, there is only one enabled output topic. - * - * Explaining some basic terms so that you know what output is the right for you: - * - * Orientation: - * - NED (North-East-Down): Azimuth will be 0 pointing to north, and increases clockwise. This is consistent with the - * azimuth used in cartography and by tourists. - * - ENU (East-North-Up): Azimuth will be 0 pointing to east, and increases counter-clockwise. This is consistent with - * REP-103 and robot_localization package. - * References for north: - * - Magnetic: points towards the magnetic north of Earth (travels in time). - * - Geographic ("true"): points towards the geographic Earth (i.e. the WGS84 North Pole). It is static in time. - * - UTM: points in the north direction on the cartesian UTM grid (similar to Geographic, but it can slightly diverge - * at the edges of UTM maps). You probably want this azimuth reference for navigation tasks in UTM coordinates. - * - * Magnetic azimuth can be computed directly from the magnetometer and IMU orientation. To compute the other two - * references, you need to provide the latitude, longitude, altitude and time in addition to the magnetometer and - * IMU orientation. These are the required inputs to compute magnetic declination and UTM grid convergence, which are - * the offsets by which geographic and UTM references differ from the magnetic. This is why this compass node subscribes - * to the GPS fix messages. Until at least a single GPS fix message is received, neither geographic- nor UTM-referenced - * data are published. If you do not have a GPS receiver, you can alternatively provide these values in parameters. - * - * For the magnetometer to work correctly, it is required to measure its bias. This node listens on the `imu/mag_bias` - * topic for this measurement, and until at least one message arrives, the node will not publish anything. If you do not - * have a node publishing the bias, you can alternatively provide it via parameters. Depending on the application, it - * may be required to re-estimate the bias from time to time even during runtime. - * - * Subscribed topics: - * - `imu/data` (`sensor_msgs/Imu`): Output from an IMU or an orientation filtering algorithm. It should have valid - * contents of `orientation` and at least roll and pitch should be estimated as - * well as possible (relative to the gravity vector). These messages should come at - * the same rate as the magnetometer data (or faster). Make sure the orientation - * is reported in ENU frame (use imu_transformer package to transform it from NED). - * - `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer measurements (bias not removed). - * - `imu/mag_bias` (`sensor_msgs/MagneticField`): Bias of the magnetometer. This value will be subtracted from the - * incoming magnetometer measurements (only the `magnetic_field` field - * is relevant). Messages on this topic do not need to come repeatedly - * if the bias does not change. - * - `gps/fix` (`sensor_msgs/NavSatFix`, optional): GPS fix messages from which the latitude, longitude, altitude and - * current year can be read. These are further used to compute - * magnetic declination and UTM grid convergence factor if requested. - * - TF: This node requires a (usually static) transform between `~frame` and the frame ID of the IMU and magnetometer - * messages. - * - * Published topics (see above for explanation): - * - `imu/mag_unbiased` (`sensor_msgs/MagneticField`, enabled by param `~publish_mag_unbiased`, off by default): - * The magnetic field measurement with bias removed. - * - * - `compass/mag/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_ned_deg`, on by default): - * Magnetic azimuth in NED in degrees (the same values you can see on touristic magnetic compasses). - * - `compass/mag/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_ned_rad`, off by default): - * Magnetic azimuth in NED in radians. - * - `compass/mag/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_mag_azimuth_ned_quat`, off by default): - * Magnetic azimuth in NED as a quaternion. - * - `compass/mag/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_mag_azimuth_ned_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards magnetic NED frame. - * - `compass/mag/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_mag_azimuth_ned_pose`, off by default): - * Magnetic azimuth in NED as a pose (translation will always be zero). - * - * - `compass/mag/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_enu_deg`, off by default): - * Magnetic azimuth in ENU in degrees. - * - `compass/mag/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_enu_rad`, off by default): - * Magnetic azimuth in ENU in radians. - * - `compass/mag/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_mag_azimuth_enu_quat`, off by default): - * Magnetic azimuth in ENU as a quaternion. - * - `compass/mag/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_mag_azimuth_enu_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards magnetic ENU frame. - * - `compass/mag/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_mag_azimuth_enu_pose`, off by default): - * Magnetic azimuth in ENU as a pose (translation will always be zero). - * - * - `compass/true/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_ned_deg`, off by default): - * Geographic ("true") azimuth in NED in degrees. - * - `compass/true/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_ned_rad`, off by default): - * Geographic ("true") azimuth in NED in radians. - * - `compass/true/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_true_azimuth_ned_quat`, off by default): - * Geographic ("true") azimuth in NED as a quaternion. - * - `compass/true/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_true_azimuth_ned_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards geographic ("true") NED frame. - * - `compass/true/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_true_azimuth_ned_pose`, off by default): - * Geographic ("true") azimuth in NED as a pose (translation will always be zero). - * - * - `compass/true/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_enu_deg`, off by default): - * Geographic ("true") azimuth in ENU in degrees. - * - `compass/true/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_enu_rad`, off by default): - * Geographic ("true") azimuth in ENU in radians. - * - `compass/true/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_true_azimuth_enu_quat`, off by default): - * Geographic ("true") azimuth in ENU as a quaternion. - * - `compass/true/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_true_azimuth_enu_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards geographic ("true") ENU frame. - * - `compass/true/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_true_azimuth_enu_pose`, off by default): - * Geographic ("true") azimuth in ENU as a pose (translation will always be zero). - * - * - `compass/utm/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_ned_deg`, off by default): - * UTM heading in NED in degrees. - * - `compass/utm/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_ned_rad`, off by default): - * UTM heading in NED in radians. - * - `compass/utm/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_utm_azimuth_ned_quat`, off by default): - * UTM heading in NED as a quaternion. - * - `compass/utm/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_utm_azimuth_ned_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards UTM NED frame. - * - `compass/utm/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_utm_azimuth_ned_pose`, off by default): - * UTM heading in NED as a pose (translation will always be zero). - * - * - `compass/utm/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_enu_deg`, off by default): - * UTM heading in ENU in degrees. - * - `compass/utm/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_enu_rad`, off by default): - * UTM heading in ENU in radians. - * - `compass/utm/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_utm_azimuth_enu_quat`, off by default): - * UTM heading in ENU as a quaternion. - * - `compass/utm/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_utm_azimuth_enu_imu`, off by default): - * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards UTM ENU frame. - * - `compass/utm/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_utm_azimuth_enu_pose`, off by default): - * UTM heading in ENU as a pose (translation will always be zero). - * - * Parameters: - * - all the `publish_*` parameters mentioned above. - * - `~frame` (string, default `base_link`): Frame into which the IMU and magnetometer data should be transformed. - * - `~low_pass_ratio` (double, default 0.95): The azimuth is filtered with a low-pass filter. This sets its - * aggressivity (0 means raw measurements, 1 means no updates). - * - `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. - * - `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. - * - `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. - * - If you specify any of the `~initial_mag_bias_*` params, the node does not need to receive the bias messages. - * - `~initial_lat` (double, no default, optional): Latitude in degrees. - * - `~initial_lon` (double, no default, optional): Longitude in degrees. - * - If you specify both `~initial_lat` and `~initial_lon`, the node does not need to receive the GPS fix messages. - * - `~initial_alt` (double, default 0): Altitude in meters (it is usually okay to omit it and use the default). - * - `~initial_year` (int, no default, optional): If set, overrides the current time for declination computation. - * - `~initial_variance` (double, default 0): Variance of the measurement used at startup (in rad^2). - * - `~magnetic_declination` (double, no default, optional, radians): If this parameter is set, the magnetic models are - * ignored and this value for declination is forced. - * This can be useful either if you know the value - * in advance or in simulation. - * - `~magnetic_models_path` (string, defaults to the pre-installed directory): Directory with WMM magnetic field - * models. You usually do not need to use other than the preinstalled models. But if you do, specify the path to - * the custom models directory here. - * - `~magnetic_model` (string, defaults to autodetection by year): Name of the magnetic field model to use. If omitted, - * an automated decision is made based on the current year (or `~initial_year`, if set). This model is used for - * computing magnetic declination. - */ -class MagnetometerCompassNodelet : public nodelet::Nodelet -{ -public: - MagnetometerCompassNodelet(); - ~MagnetometerCompassNodelet() override; - -protected: - void onInit() override; - - //! \brief Joint callback when IMU and magnetometer messages are received. - //! \param[in] imu IMU data. Only `orientation` is relevant, and it should contain filtered absolute orientation. - //! \param[in] mag Magnetometer data (biased). - void imuMagCb(const sensor_msgs::Imu& imu, const sensor_msgs::MagneticField& mag); - - //! \brief Callback for GPS fix (so that the node can compute magnetic declination and UTM grid convergence). - //! \param[in] fix The fix message. Only `latitude`, `longitude`, `altitude` and `header.stamp` are relevant. - void fixCb(const sensor_msgs::NavSatFix& fix); - - //! \brief Callback for magnetometer bias. - //! \param[in] bias The bias value. Only the values in `magnetic_field` are relevant. - void magBiasCb(const sensor_msgs::MagneticField& bias); - - /** - * \brief Get the best magnetic model for the given date. - * \param[in] date The date in question. - * \return Name of the best magnetic model. If forcedMagneticModelName is non-empty, this value is always returned. - */ - virtual std::string getBestMagneticModel(const ros::Time& date) const; - - /** - * \brief Get the value of magnetic declination on the provided place on Earth. - * \param[in] fix The place for which declination is queried. - * \return The magnetic declination in radians. Zero if something fails. - */ - virtual double getMagneticDeclination(const sensor_msgs::NavSatFix& fix) const; - - /** - * \brief Update the `variance` variable that tells uncertainty of the azimuth measurement. - */ - virtual void updateVariance(); - - /** - * \brief Convert the ROS stamp to a calendar year. - * \param[in] date The ROS stamp. - * \return Year of the ROS stamp. Returns 0 if some failure occurs. - */ - static uint32_t getYear(const ros::Time& date); - - //! \brief TF frame in which the compass should be expressed. Usually base_link. - std::string frame; - - //! \brief Current azimuth in NED. If invalid, no azimuth has been determined yet. - tf2::Quaternion magAzimuth {0, 0, 0, 0}; - - //! \brief Variance of the current azimuth (i.e. stddev squared). - double variance {0}; - - //! \brief Last received IMU message. - sensor_msgs::Imu lastImu; - - //! \brief Last received IMU message transformed to `frame`. - sensor_msgs::Imu lastImuInBody; - - //! \brief Last received magnetic field message. - sensor_msgs::MagneticField lastMag; - - //! \brief Last received magnetic field message with bias removed and transformed to `frame`. - sensor_msgs::MagneticField lastMagUnbiasedInBody; - - //! \brief Last computed magnetic declination. If invalid, no declination has been computed yet. - tf2::Quaternion lastMagneticDeclination {0, 0, 0, 0}; - - //! \brief Whether the magnetic declination value has been forced by the user (in such case, do not update it). - bool magneticDeclinationForced {false}; - - //! \brief Last received GPS fix. - sensor_msgs::NavSatFix lastFix; - - //! \brief Whether magnetometer bias has already been established either from subscriber or initial parameters. - bool hasMagBias{false}; - - //! \brief Last received value of magnetometer bias. - tf2::Vector3 lastMagBias {0, 0, 0}; - - //! \brief Scaling factor of magnetometer measurements (optional). - tf2::Matrix3x3 lastMagScale {1, 0, 0, 0, 1, 0, 0, 0, 1}; - - //! \brief Aggresivity of the azimuth low-pass filter. 0 means raw measurements, 1 means no updates. - double magAzimuthLowPass{0.95}; - - //! \brief Path to the folder with magnetic field models. - std::string magneticModelsPath{}; - - //! \brief If the user forces a magnetic model, this is its name. - std::string forcedMagneticModelName{}; - - //! TF buffer (for transforming IMU data to `frame`). - tf2_ros::Buffer tf; - - //! \brief PIMPL data - std::unique_ptr data; -}; - -} diff --git a/magnetometer_compass/launch/compass.launch b/magnetometer_compass/launch/compass.launch new file mode 100644 index 0000000..3f17e13 --- /dev/null +++ b/magnetometer_compass/launch/compass.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/magnetometer_compass/nodelets.xml b/magnetometer_compass/nodelets.xml index 071d586..51ae892 100644 --- a/magnetometer_compass/nodelets.xml +++ b/magnetometer_compass/nodelets.xml @@ -1,6 +1,17 @@ - - - Compass based on a 3-axis magnetometer and odometry pose readings. - - + + + + + + + Compass based on a 3-axis magnetometer and odometry pose readings. + + + + + Azimuth visualization as a Pose pointing to North. + + + \ No newline at end of file diff --git a/magnetometer_compass/nodelets/magnetometer_compass_nodelet.cpp b/magnetometer_compass/nodelets/magnetometer_compass_nodelet.cpp new file mode 100644 index 0000000..4d5be51 --- /dev/null +++ b/magnetometer_compass/nodelets/magnetometer_compass_nodelet.cpp @@ -0,0 +1,572 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_compass +{ +using Az = compass_msgs::Azimuth; +using Quat = geometry_msgs::QuaternionStamped; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Imu = sensor_msgs::Imu; +using Field = sensor_msgs::MagneticField; + +typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + +struct AzimuthPublishersConfigForOrientation : public cras::HasLogger +{ + std::shared_ptr converter; + + ros::Publisher quatPub; + ros::Publisher imuPub; + ros::Publisher posePub; + ros::Publisher radPub; + ros::Publisher degPub; + + bool publishQuat{false}; + bool publishImu{false}; + bool publishPose{false}; + bool publishRad{false}; + bool publishDeg{false}; + + bool publish{false}; + + explicit AzimuthPublishersConfigForOrientation(const cras::LogHelperPtr& log); + + void init( + ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::shared_ptr& converter, + const std::string& paramPrefix, const std::string& topicPrefix, uint8_t reference, uint8_t orientation, + const std::string& referenceStr, const std::string& orientationStr); + + void publishAzimuths(const Az& azimuthRad, const Imu& imuInBody); +}; + +struct AzimuthPublishersConfig : public cras::HasLogger +{ + std::shared_ptr converter; + + AzimuthPublishersConfigForOrientation ned; + AzimuthPublishersConfigForOrientation enu; + + bool publish{false}; + + const tf2::Quaternion nedToEnu{-M_SQRT2 / 2, -M_SQRT2 / 2, 0, 0}; + const tf2::Quaternion enuToNed{this->nedToEnu.inverse()}; + + explicit AzimuthPublishersConfig(const cras::LogHelperPtr& log); + + void init( + ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::shared_ptr& converter, + const std::string& paramPrefix, const std::string& topicPrefix, uint8_t reference, const std::string& referenceStr); + + void publishAzimuths(const Az& nedAzimuth, const Imu& imuInBody); +}; + +/** + * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. + * + * Because there is no well established Azimuth message in ROS, this node publishes our custom compass_msgs/Azimuth + * as well as a few other formats that are capable of carrying orientation information. It also offers the azimuth + * values in both radians and degrees, because radians are the ROS standard, while degrees are more used in the + * geographic area. There are tens of possible combinations of the output data formats, so each of the published + * topics has a boolean parameter that enables it. By default, there is only one enabled output topic. + * + * Explaining some basic terms so that you know what output is the right for you: + * + * Orientation: + * - NED (North-East-Down): Azimuth will be 0 pointing to north, and increases clockwise. This is consistent with the + * azimuth used in cartography and by tourists. + * - ENU (East-North-Up): Azimuth will be 0 pointing to east, and increases counter-clockwise. This is consistent with + * REP-103 and robot_localization package. + * References for north: + * - Magnetic: points towards the magnetic north of Earth (travels in time). + * - Geographic ("true"): points towards the geographic Earth (i.e. the WGS84 North Pole). It is static in time. + * - UTM: points in the north direction on the cartesian UTM grid (similar to Geographic, but it can slightly diverge + * at the edges of UTM maps). You probably want this azimuth reference for navigation tasks in UTM coordinates. + * + * Magnetic azimuth can be computed directly from the magnetometer and IMU orientation. To compute the other two + * references, you need to provide the latitude, longitude, altitude and time in addition to the magnetometer and + * IMU orientation. These are the required inputs to compute magnetic declination and UTM grid convergence, which are + * the offsets by which geographic and UTM references differ from the magnetic. This is why this compass node subscribes + * to the GPS fix messages. Until at least a single GPS fix message is received, neither geographic- nor UTM-referenced + * data are published. If you do not have a GPS receiver, you can alternatively provide these values in parameters. + * + * For the magnetometer to work correctly, it is required to measure its bias. This node listens on the `imu/mag_bias` + * topic for this measurement, and until at least one message arrives, the node will not publish anything. If you do not + * have a node publishing the bias, you can alternatively provide it via parameters. Depending on the application, it + * may be required to re-estimate the bias from time to time even during runtime. + * + * Subscribed topics: + * - `imu/data` (`sensor_msgs/Imu`): Output from an IMU or an orientation filtering algorithm. It should have valid + * contents of `orientation` and at least roll and pitch should be estimated as + * well as possible (relative to the gravity vector). These messages should come at + * the same rate as the magnetometer data (or faster). Make sure the orientation + * is reported in ENU frame (use imu_transformer package to transform it from NED). + * - `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer raw measurements (bias not removed) (disabled by param + * `~subscribe_mag_unbiased`). + * - `imu/mag_bias` (`sensor_msgs/MagneticField`): Bias of the magnetometer. This value will be subtracted from the + * incoming magnetometer measurements. Messages on this topic do not + * need to come repeatedly if the bias does not change. Disabled by + * param `~subscribe_mag_unbiased`. + * - `imu/mag_unbiased` (`sensor_msgs/MagneticField`): 3-axis magnetometer unbiased measurements (enabled by param + * `~subscribe_mag_unbiased`). + * - `gps/fix` (`sensor_msgs/NavSatFix`, optional): GPS fix messages from which the latitude, longitude, altitude and + * current year can be read. These are further used to compute + * magnetic declination and UTM grid convergence factor if requested. + * - TF: This node requires a (usually static) transform between `~frame` and the frame ID of the IMU and magnetometer + * messages. + * + * Published topics (see above for explanation): + * - `imu/mag_unbiased` (`sensor_msgs/MagneticField`, enabled by param `~publish_mag_unbiased`, off by default): + * The magnetic field measurement with bias removed. + * + * - `compass/mag/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_ned_deg`, on by default): + * Magnetic azimuth in NED in degrees (the same values you can see on touristic magnetic compasses). + * - `compass/mag/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_ned_rad`, off by default): + * Magnetic azimuth in NED in radians. + * - `compass/mag/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_mag_azimuth_ned_quat`, off by default): + * Magnetic azimuth in NED as a quaternion. + * - `compass/mag/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_mag_azimuth_ned_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards magnetic NED frame. + * - `compass/mag/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_mag_azimuth_ned_pose`, off by default): + * Magnetic azimuth in NED as a pose (translation will always be zero). + * + * - `compass/mag/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_enu_deg`, off by default): + * Magnetic azimuth in ENU in degrees. + * - `compass/mag/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_mag_azimuth_enu_rad`, off by default): + * Magnetic azimuth in ENU in radians. + * - `compass/mag/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_mag_azimuth_enu_quat`, off by default): + * Magnetic azimuth in ENU as a quaternion. + * - `compass/mag/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_mag_azimuth_enu_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards magnetic ENU frame. + * - `compass/mag/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_mag_azimuth_enu_pose`, off by default): + * Magnetic azimuth in ENU as a pose (translation will always be zero). + * + * - `compass/true/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_ned_deg`, off by default): + * Geographic ("true") azimuth in NED in degrees. + * - `compass/true/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_ned_rad`, off by default): + * Geographic ("true") azimuth in NED in radians. + * - `compass/true/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_true_azimuth_ned_quat`, off by default): + * Geographic ("true") azimuth in NED as a quaternion. + * - `compass/true/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_true_azimuth_ned_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards geographic ("true") NED frame. + * - `compass/true/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_true_azimuth_ned_pose`, off by default): + * Geographic ("true") azimuth in NED as a pose (translation will always be zero). + * + * - `compass/true/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_enu_deg`, off by default): + * Geographic ("true") azimuth in ENU in degrees. + * - `compass/true/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_true_azimuth_enu_rad`, off by default): + * Geographic ("true") azimuth in ENU in radians. + * - `compass/true/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_true_azimuth_enu_quat`, off by default): + * Geographic ("true") azimuth in ENU as a quaternion. + * - `compass/true/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_true_azimuth_enu_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards geographic ("true") ENU frame. + * - `compass/true/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_true_azimuth_enu_pose`, off by default): + * Geographic ("true") azimuth in ENU as a pose (translation will always be zero). + * + * - `compass/utm/ned/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_ned_deg`, off by default): + * UTM heading in NED in degrees. + * - `compass/utm/ned/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_ned_rad`, off by default): + * UTM heading in NED in radians. + * - `compass/utm/ned/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_utm_azimuth_ned_quat`, off by default): + * UTM heading in NED as a quaternion. + * - `compass/utm/ned/imu` (`sensor_msgs/Imu`, enabled by param `~publish_utm_azimuth_ned_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards UTM NED frame. + * - `compass/utm/ned/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_utm_azimuth_ned_pose`, off by default): + * UTM heading in NED as a pose (translation will always be zero). + * + * - `compass/utm/enu/deg` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_enu_deg`, off by default): + * UTM heading in ENU in degrees. + * - `compass/utm/enu/rad` (`compass_msgs/Azimuth`, enabled by param `~publish_utm_azimuth_enu_rad`, off by default): + * UTM heading in ENU in radians. + * - `compass/utm/enu/quat` (`geometry_msgs/QuaternionStamped`, enabled by param `~publish_utm_azimuth_enu_quat`, off by default): + * UTM heading in ENU as a quaternion. + * - `compass/utm/enu/imu` (`sensor_msgs/Imu`, enabled by param `~publish_utm_azimuth_enu_imu`, off by default): + * The incoming IMU message rotated in yaw such that its frame becomes georeferenced towards UTM ENU frame. + * - `compass/utm/enu/pose` (`geometry_msgs/PoseWithCovarianceStamped`, enabled by param `~publish_utm_azimuth_enu_pose`, off by default): + * UTM heading in ENU as a pose (translation will always be zero). + * + * Parameters: + * - All the `publish_*` parameters mentioned above. + * - Please note that you cannot combine both `~subscribe_mag_unbiased` and `~publish_mag_unbiased` set to true. + * Such configuration is invalid and the node will not start. + * - `~frame` (string, default `base_link`): Frame into which the IMU and magnetometer data should be transformed. + * - `~low_pass_ratio` (double, default 0.95): The azimuth is filtered with a low-pass filter. This sets its + * aggressivity (0 means raw measurements, 1 means no updates). + * - `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. + * - `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. + * - `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. + * - If you specify any of the `~initial_mag_bias_*` params, the node does not need to receive the bias messages. + * - `~initial_lat` (double, no default, optional): Latitude in degrees. + * - `~initial_lon` (double, no default, optional): Longitude in degrees. + * - If you specify both `~initial_lat` and `~initial_lon`, the node does not need to receive the GPS fix messages. + * - `~initial_alt` (double, default 0): Altitude in meters (it is usually okay to omit it and use the default). + * - `~initial_year` (int, no default, optional): If set, overrides the current time for declination computation. + * - `~initial_variance` (double, default 0): Variance of the measurement used at startup (in rad^2). + * - `~magnetic_declination` (double, no default, optional, radians): If this parameter is set, the magnetic models are + * ignored and this value for declination is forced. + * This can be useful either if you know the value + * in advance or in simulation. + * - `~magnetic_models_path` (string, defaults to the pre-installed directory): Directory with WMM magnetic field + * models. You usually do not need to use other than the preinstalled models. But if you do, specify the path to + * the custom models directory here. + * - `~magnetic_model` (string, defaults to autodetection by year): Name of the magnetic field model to use. If omitted, + * an automated decision is made based on the current year (or `~initial_year`, if set). This model is used for + * computing magnetic declination. + */ +class MagnetometerCompassNodelet : public cras::Nodelet +{ +public: + MagnetometerCompassNodelet(); + ~MagnetometerCompassNodelet() override; + +protected: + void onInit() override; + + //! \brief Joint callback when IMU and magnetometer messages are received. + //! \param[in] imu IMU data. Only `orientation` is relevant, and it should contain filtered absolute orientation. + //! \param[in] mag Magnetometer data (biased). + void imuMagCb(const sensor_msgs::Imu& imu, const sensor_msgs::MagneticField& mag); + + //! \brief Callback for GPS fix (so that the node can compute magnetic declination and UTM grid convergence). + //! \param[in] fix The fix message. Only `latitude`, `longitude`, `altitude` and `header.stamp` are relevant. + void fixCb(const sensor_msgs::NavSatFix& fix); + + //! \brief TF frame in which the compass should be expressed. Usually base_link. + std::string frame; + + std::shared_ptr converter; + std::shared_ptr compass; + std::unique_ptr> imuSub; + std::unique_ptr> magSub; + std::unique_ptr> magBiasSub; + std::unique_ptr> magUnbiasedSub; + std::unique_ptr magBiasRemoverFilter; + std::unique_ptr> syncSub; + ros::Subscriber fixSub; + + ros::Publisher magUnbiasedPub; + bool publishMagUnbiased{false}; + bool subscribeMagUnbiased{false}; + + AzimuthPublishersConfig magPublishers; + AzimuthPublishersConfig truePublishers; + AzimuthPublishersConfig utmPublishers; +}; + +MagnetometerCompassNodelet::MagnetometerCompassNodelet() : + magPublishers(this->log), truePublishers(this->log), utmPublishers(this->log) +{ +} + +MagnetometerCompassNodelet::~MagnetometerCompassNodelet() = default; + +void MagnetometerCompassNodelet::onInit() +{ + auto nh = this->getNodeHandle(); + auto pnh = this->getPrivateNodeHandle(); + + auto params = this->privateParams(); + + this->frame = params->getParam("frame", "base_link"); + + const auto strict = params->getParam("strict", true); + this->converter = std::make_shared(this->getLogger(), strict); + this->converter->configFromParams(*params); + + this->compass = std::make_shared(this->log, this->frame, this->getBufferPtr()); + this->compass->configFromParams(*params); + + this->publishMagUnbiased = params->getParam("publish_mag_unbiased", this->publishMagUnbiased); + this->subscribeMagUnbiased = params->getParam("subscribe_mag_unbiased", this->subscribeMagUnbiased); + + if (this->publishMagUnbiased && this->subscribeMagUnbiased) + throw std::runtime_error("Cannot simultaneously subscribe and publish unbiased magnetometer."); + + // Set default publishers + this->magPublishers.ned.publishDeg = true; + + bool publish = this->publishMagUnbiased; + + ros::NodeHandle compassNh(nh, "compass"); + // Read the publish_* parameters and set up the selected publishers + this->magPublishers.init(compassNh, pnh, this->converter, "publish", "", Az::REFERENCE_MAGNETIC, "mag"); + publish |= this->magPublishers.publish; + this->truePublishers.init(compassNh, pnh, this->converter, "publish", "", Az::REFERENCE_GEOGRAPHIC, "true"); + publish |= this->truePublishers.publish; + this->utmPublishers.init(compassNh, pnh, this->converter, "publish", "", Az::REFERENCE_UTM, "utm"); + publish |= this->utmPublishers.publish; + + if (!publish) + CRAS_WARN("No publishers have been requested. Please, set one of the publish_* parameters to true."); + + ros::NodeHandle imuNh(nh, "imu"); + + if (this->publishMagUnbiased) + this->magUnbiasedPub = imuNh.advertise("mag_unbiased", 10); + + this->imuSub = std::make_unique>(imuNh, "data", 100); + + // Check if we should try to unbias the magnetometer ourselves or if you already got it unbiased on the input. + if (this->subscribeMagUnbiased) + { + this->magSub = std::make_unique>(imuNh, "mag_unbiased", 100); + this->syncSub = std::make_unique>( + SyncPolicy(200), *this->imuSub, *this->magSub); + } + else + { + this->magSub = std::make_unique>(imuNh, "mag", 100); + this->magBiasSub = std::make_unique>(imuNh, "mag_bias", 10); + this->magBiasRemoverFilter = std::make_unique( + this->getLogger(), *this->magSub, *this->magBiasSub); + this->magBiasRemoverFilter->configFromParams(*params); + + this->syncSub = std::make_unique>( + SyncPolicy(200), *this->imuSub, *this->magBiasRemoverFilter); + } + this->syncSub->registerCallback(&MagnetometerCompassNodelet::imuMagCb, this); + + this->fixSub = nh.subscribe("gps/fix", 10, &MagnetometerCompassNodelet::fixCb, this); +} + +AzimuthPublishersConfigForOrientation::AzimuthPublishersConfigForOrientation(const cras::LogHelperPtr& log) : + cras::HasLogger(log) +{ +} + +void AzimuthPublishersConfigForOrientation::init( + ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::shared_ptr& converter, + const std::string& paramPrefix, const std::string& topicPrefix, const uint8_t reference, const uint8_t orientation, + const std::string& referenceStr, const std::string& orientationStr) +{ + this->converter = converter; + + auto prefix = paramPrefix + "_" + referenceStr + "_azimuth_" + orientationStr + "_"; + this->publishQuat = pnh.param(prefix + "quat", this->publishQuat); + this->publishImu = pnh.param(prefix + "imu", this->publishImu); + this->publishPose = pnh.param(prefix + "pose", this->publishPose); + this->publishRad = pnh.param(prefix + "rad", this->publishRad); + this->publishDeg = pnh.param(prefix + "deg", this->publishDeg); + + this->publish = this->publishQuat || this->publishImu || this->publishPose || this->publishDeg || this->publishRad; + + using compass_conversions::getAzimuthTopicSuffix; + + prefix = cras::appendIfNonEmpty(topicPrefix, "/"); + if (this->publishQuat) + this->quatPub = nh.advertise(prefix + getAzimuthTopicSuffix(Az::UNIT_RAD, orientation, reference), 10); + if (this->publishImu) + this->imuPub = nh.advertise(prefix + getAzimuthTopicSuffix(Az::UNIT_RAD, orientation, reference), 10); + if (this->publishPose) + this->posePub = nh.advertise(prefix + getAzimuthTopicSuffix(Az::UNIT_RAD, orientation, reference), 10); + if (this->publishRad) + this->radPub = nh.advertise(prefix + getAzimuthTopicSuffix(Az::UNIT_RAD, orientation, reference), 10); + if (this->publishDeg) + this->degPub = nh.advertise(prefix + getAzimuthTopicSuffix(Az::UNIT_DEG, orientation, reference), 10); +} + +AzimuthPublishersConfig::AzimuthPublishersConfig(const cras::LogHelperPtr& log) : + cras::HasLogger(log), ned(log), enu(log) +{ +} + +void AzimuthPublishersConfig::init( + ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::shared_ptr& converter, + const std::string& paramPrefix, const std::string& topicPrefix, + const uint8_t reference, const std::string& referenceStr) +{ + this->converter = converter; + this->ned.init(nh, pnh, converter, paramPrefix, topicPrefix, reference, Az::ORIENTATION_NED, referenceStr, "ned"); + this->enu.init(nh, pnh, converter, paramPrefix, topicPrefix, reference, Az::ORIENTATION_ENU, referenceStr, "enu"); + this->publish = this->ned.publish || this->enu.publish; +} + +void MagnetometerCompassNodelet::imuMagCb(const Imu& imu, const Field& magUnbiased) +{ + if (this->publishMagUnbiased) + this->magUnbiasedPub.publish(magUnbiased); + + const auto maybeAzimuth = this->compass->computeAzimuth(imu, magUnbiased); + if (!maybeAzimuth.has_value()) + { + CRAS_ERROR_DELAYED_THROTTLE(1.0, "%s", maybeAzimuth.error().c_str()); + return; + } + + Imu imuInBody; + try + { + // No timeout because computeAzimuth() has already waited for this exact transform + this->getBuffer().transform(imu, imuInBody, this->frame); + } + catch (const tf2::TransformException& e) + { + CRAS_ERROR_DELAYED_THROTTLE(1.0, + "Could not transform IMU data to frame %s because: %s", this->frame.c_str(), e.what()); + return; + } + + const auto& nedAzimuthMsg = *maybeAzimuth; + this->magPublishers.publishAzimuths(nedAzimuthMsg, imuInBody); + + if (this->truePublishers.publish) + { + const auto maybeTrueNedAzimuthMsg = this->converter->convertAzimuth( + nedAzimuthMsg, nedAzimuthMsg.unit, nedAzimuthMsg.orientation, Az::REFERENCE_GEOGRAPHIC); + if (maybeTrueNedAzimuthMsg) + this->truePublishers.publishAzimuths(*maybeTrueNedAzimuthMsg, imuInBody); + else + CRAS_ERROR_DELAYED_THROTTLE(1.0, "%s", maybeTrueNedAzimuthMsg.error().c_str()); + } + + if (this->utmPublishers.publish) + { + const auto maybeUTMNedAzimuthMsg = this->converter->convertAzimuth( + nedAzimuthMsg, nedAzimuthMsg.unit, nedAzimuthMsg.orientation, Az::REFERENCE_UTM); + if (maybeUTMNedAzimuthMsg.has_value()) + this->utmPublishers.publishAzimuths(*maybeUTMNedAzimuthMsg, imuInBody); + else + CRAS_ERROR_DELAYED_THROTTLE(1.0, "%s", maybeUTMNedAzimuthMsg.error().c_str()); + } +} + +void AzimuthPublishersConfig::publishAzimuths(const Az& nedAzimuth, const Imu& imuInBody) +{ + if (!this->publish) + return; + + if (this->ned.publish) + { + auto imuNed = imuInBody; // If IMU message should not be published, we fake it here with the ENU-referenced one + if (this->ned.publishImu) + { + geometry_msgs::TransformStamped tf; + tf.header.stamp = imuInBody.header.stamp; + tf.header.frame_id = imuInBody.header.frame_id + "_ned"; + tf2::convert(this->enuToNed, tf.transform.rotation); + tf2::doTransform(imuInBody, imuNed, tf); + } + this->ned.publishAzimuths(nedAzimuth, imuNed); + } + + if (this->enu.publish) + { + // Rotate to ENU + auto maybeEnuAzimuth = this->converter->convertAzimuth( + nedAzimuth, nedAzimuth.unit, Az::ORIENTATION_ENU, nedAzimuth.reference); + + if (maybeEnuAzimuth.has_value()) + this->enu.publishAzimuths(*maybeEnuAzimuth, imuInBody); + else + CRAS_ERROR_THROTTLE(1.0, "Could not convert from NED to ENU: %s", maybeEnuAzimuth.error().c_str()); + } +} + +void AzimuthPublishersConfigForOrientation::publishAzimuths(const Az& azimuthRad, const Imu& imuInBody) +{ + if (this->publishQuat) + { + const auto maybeQuat = this->converter->convertToQuaternion(azimuthRad); + if (maybeQuat.has_value()) + this->quatPub.publish(*maybeQuat); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybeQuat.error().c_str()); + } + + if (this->publishImu) + { + const auto maybeQuat = this->converter->convertToQuaternion(azimuthRad); + if (!maybeQuat.has_value()) + { + CRAS_ERROR_THROTTLE(1.0, "%s", maybeQuat.error().c_str()); + } + else + { + // The IMU message comes in an arbitrarily-referenced frame, and we adjust its yaw to become georeferenced. + double azimuthYaw = cras::getYaw(maybeQuat->quaternion); + + tf2::Quaternion imuRot; + tf2::convert(imuInBody.orientation, imuRot); + double roll, pitch, yaw; + cras::getRPY(imuRot, roll, pitch, yaw); + + tf2::Quaternion desiredRot; + desiredRot.setRPY(roll, pitch, azimuthYaw); + + const auto diffRot = desiredRot.inverse() * imuRot; + + sensor_msgs::Imu imuMsg; + + geometry_msgs::TransformStamped tf; + tf.header = imuInBody.header; + tf2::convert(diffRot, tf.transform.rotation); + tf2::doTransform(imuInBody, imuMsg, tf); + + imuMsg.orientation_covariance[8] = azimuthRad.variance; + + this->imuPub.publish(imuMsg); + } + } + + if (this->publishPose) + { + const auto maybePose = this->converter->convertToPose(azimuthRad); + if (maybePose.has_value()) + this->posePub.publish(*maybePose); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybePose.error().c_str()); + } + + if (this->publishRad) + { + this->radPub.publish(azimuthRad); + } + + if (this->publishDeg) + { + const auto maybeAzimuthDeg = this->converter->convertAzimuth( + azimuthRad, Az::UNIT_DEG, azimuthRad.orientation, azimuthRad.reference); + if (maybeAzimuthDeg.has_value()) + this->degPub.publish(*maybeAzimuthDeg); + else + CRAS_ERROR_THROTTLE(1.0, "%s", maybeAzimuthDeg.error().c_str()); + } +} + +void MagnetometerCompassNodelet::fixCb(const sensor_msgs::NavSatFix& fix) +{ + this->converter->setNavSatPos(fix); +} + +} + +PLUGINLIB_EXPORT_CLASS(magnetometer_compass::MagnetometerCompassNodelet, nodelet::Nodelet) diff --git a/magnetometer_compass/nodelets/visualize_azimuth_nodelet.cpp b/magnetometer_compass/nodelets/visualize_azimuth_nodelet.cpp new file mode 100644 index 0000000..b84c8fb --- /dev/null +++ b/magnetometer_compass/nodelets/visualize_azimuth_nodelet.cpp @@ -0,0 +1,159 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Visualize Azimuth as a PoseWithCovarianceStamped pointing to North. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_compass +{ +using Az = compass_msgs::Azimuth; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Fix = sensor_msgs::NavSatFix; +using Zone = std_msgs::Int32; + +/** + * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. + * + * Subscribed topics: + * - `~azimuth` (multiple types supported): The azimuth to visualize. Any type supported by compass_conversions package + * can be used: `compass_msgs/Azimuth`, `geometry_msgs/Quaternion`, + * `geometry_msgs/PoseWithCovarianceStamped` or `sensor_msgs/Imu`. If other + * types than `compass_msgs/Azimuth` are used, either the resolved topic name + * must contain the azimuth type identification (e.g. end with `mag/enu/imu`), + * or you must provide parameters `~input_reference` and `~input_orientation`. + * - `gps/fix` (`sensor_msgs/NavSatFix`, optional): GPS fix messages from which the latitude, longitude, altitude and + * current year can be read. These are further used to compute + * magnetic declination and UTM grid convergence factor. + * - `utm_zone` (`std_msgs/Int32`, optional): Optional UTM zone updates. + * + * Published topics (see above for explanation): + * - `~azimuth_vis` (`sensor_msgs/MagneticField`, enabled by param `~publish_mag_unbiased`, off by default): + * The magnetic field measurement with bias removed. + * + * Parameters: + * - `max_rate` (double, optional): If specified, visualization messages frequency will be at most this value [Hz]. + * - `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. + * - `~input_orientation` (str, 'enu' or 'ned', default: unspecified): ENU or NED orientation to be used to interpret + * input messages (in case orientation cannot be + * derived either from message contents or topic + * name). + * - `~input_reference` (str, 'magnetic', 'geographic' or 'UTM', default: no change): North reference to be used to + * interpret input messages (in case + * reference cannot be derived either + * from message contents or topic + * name). + * - `~input_variance` (double, optional, rad^2): If specified, this variance will be used in the output messages + * if variance cannot be determined from the input messages (e.g. for + * `QuaternionStamped`). + */ +class VisualizeAzimuthNodelet : public cras::Nodelet +{ +protected: + void onInit() override; + + /** + * \brief Callback Azimuth messages are received. + * \param[in] azimuth The Azimuth converted to pose. + */ + void azimuthCb(const Az& azimuth); + + std::unique_ptr rateLimiter; + std::shared_ptr converter; + std::unique_ptr azSub; + std::unique_ptr> fixSub; + std::unique_ptr> zoneSub; + std::unique_ptr filter; + + ros::Publisher visPub; +}; + +void VisualizeAzimuthNodelet::onInit() +{ + auto nh = this->getNodeHandle(); + auto pnh = this->getPrivateNodeHandle(); + + const auto params = this->privateParams(); + if (params->hasParam("max_rate")) + this->rateLimiter = std::make_unique( + params->getParam("max_rate", cras::nullopt)); + + this->converter = std::make_shared(this->getLogger(), true); + this->converter->configFromParams(*params); + + this->visPub = pnh.advertise("azimuth_vis", 10); + + this->azSub = std::make_unique(this->log, pnh, "azimuth", 100); + this->azSub->configFromParams(*params); + this->fixSub = std::make_unique>(nh, "gps/fix", 10); + this->zoneSub = std::make_unique>(nh, "utm_zone", 10); + this->filter = std::make_unique( + this->log, this->converter, *this->azSub, *this->fixSub, *this->zoneSub, + Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + this->filter->registerCallback(&VisualizeAzimuthNodelet::azimuthCb, this); + + CRAS_INFO("Visualizing azimuth messages from [%s] on topic [%s]", + this->azSub->getTopic().c_str(), this->visPub.getTopic().c_str()); +} + +void VisualizeAzimuthNodelet::azimuthCb(const Az& azimuthEast) +{ + if (this->rateLimiter != nullptr && !this->rateLimiter->shouldPublish(azimuthEast.header.stamp)) + return; + + auto azimuthNorth = azimuthEast; + azimuthNorth.azimuth -= M_PI / 2; + + const auto maybePose = this->converter->convertToPose(azimuthNorth); + if (!maybePose.has_value()) + { + CRAS_ERROR_DELAYED_THROTTLE(10.0, "Visualizing azimuth failed: %s", maybePose.error().c_str()); + return; + } + + auto pose = *maybePose; + // Invert the orientation. Normally, azimuth tells us the direction of local X axis from North. However, for + // visualization, we want to get a pose in local frame that points to North. + tf2::Quaternion q; + tf2::convert(pose.pose.pose.orientation, q); + tf2::convert(q.inverse(), pose.pose.pose.orientation); + // The infinite covariances yielded by converter do not play well with RViz + pose.pose.covariance[0 * 6 + 0] = 0; + pose.pose.covariance[1 * 6 + 1] = 0; + pose.pose.covariance[2 * 6 + 2] = 0; + this->visPub.publish(pose); +} + +} + +PLUGINLIB_EXPORT_CLASS(magnetometer_compass::VisualizeAzimuthNodelet, nodelet::Nodelet) diff --git a/magnetometer_compass/nodes/visualize_azimuth b/magnetometer_compass/nodes/visualize_azimuth deleted file mode 100755 index 4b0a0c7..0000000 --- a/magnetometer_compass/nodes/visualize_azimuth +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -# Visualize compass_msgs/Azimuth messages as a pose. -# Author: Martin Pecka -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague - -from math import radians - -import rospy -from tf.transformations import quaternion_from_euler, quaternion_inverse - -from compass_msgs.msg import Azimuth -from geometry_msgs.msg import PoseWithCovarianceStamped - - -def cb(msg): - """Process the incoming message. - - :param Azimuth msg: The azimuth message. - """ - pose = PoseWithCovarianceStamped() - pose.header = msg.header - azimuth_rad = msg.azimuth - if msg.unit == Azimuth.UNIT_DEG: - azimuth_rad = radians(azimuth_rad) - q = quaternion_inverse(quaternion_from_euler(0, 0, azimuth_rad)) - pose.pose.pose.orientation.x = q[0] - pose.pose.pose.orientation.y = q[1] - pose.pose.pose.orientation.z = q[2] - pose.pose.pose.orientation.w = q[3] - pose.pose.covariance[-1] = msg.variance - pub.publish(pose) - - -if __name__ == '__main__': - rospy.init_node('visualize_azimuth') - - pub = rospy.Publisher('~azimuth_vis', PoseWithCovarianceStamped, queue_size=10) - sub = rospy.Subscriber('~azimuth', Azimuth, cb, queue_size=10) - - rospy.spin() diff --git a/magnetometer_compass/package.xml b/magnetometer_compass/package.xml index 817a9a7..57bec1d 100644 --- a/magnetometer_compass/package.xml +++ b/magnetometer_compass/package.xml @@ -1,46 +1,57 @@ + + magnetometer_compass 1.0.3 - Compass based on a 3-axis magnetometer, attitude readings and possibly also GPS. + Compass based on a 3-axis magnetometer, attitude readings and possibly also GNSS. Martin Pecka Martin Pecka - BSD + BSD + + https://github.com/ctu-vras/compass + https://github.com/ctu-vras/compass/issues + https://wiki.ros.org/magnetometer_compass catkin + compass_msgs + cras_cpp_common + sensor_msgs + tf2 + angles - compass_msgs - cras_cpp_common - geographiclib + compass_conversions geometry_msgs imu_transformer imu_transformer + magnetometer_pipeline message_filters nodelet roscpp - roslib - sensor_msgs - tf2 tf2_geometry_msgs tf2_ros - compass_msgs - geographiclib + angles + compass_conversions geometry_msgs imu_transformer + magnetometer_pipeline message_filters nodelet roscpp - roslib - sensor_msgs - tf2 tf2_geometry_msgs tf2_ros + python-catkin-lint + python3-catkin-lint + roslaunch + roslint + rostest + - + diff --git a/magnetometer_compass/src/magnetometer_compass.cpp b/magnetometer_compass/src/magnetometer_compass.cpp new file mode 100644 index 0000000..c8aa311 --- /dev/null +++ b/magnetometer_compass/src/magnetometer_compass.cpp @@ -0,0 +1,158 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Convert magnetometer and IMU measurements to azimuth. + * \author Martin Pecka + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_compass +{ +using Az = compass_msgs::Azimuth; +using Imu = sensor_msgs::Imu; +using Field = sensor_msgs::MagneticField; + +struct MagnetometerCompassPrivate +{ + std::shared_ptr tf; + std::string frame; + cras::optional lastAzimuth; + double variance {0.0}; + double initialVariance {0.0}; + double lowPassRatio {0.95}; +}; + +MagnetometerCompass::MagnetometerCompass( + const cras::LogHelperPtr& log, const std::string& frame, const std::shared_ptr& tf) : + MagnetometerCompass(log, frame, std::make_shared(tf)) +{ +} + +MagnetometerCompass::MagnetometerCompass(const cras::LogHelperPtr& log, const std::string& frame, + const std::shared_ptr& tf) : + cras::HasLogger(log), data(new MagnetometerCompassPrivate{}) +{ + this->data->tf = tf; + this->data->frame = frame; +} + +MagnetometerCompass::~MagnetometerCompass() = default; + +void MagnetometerCompass::configFromParams(const cras::BoundParamHelper& params) +{ + this->data->variance = this->data->initialVariance = params.getParam("initial_variance", this->data->variance); + this->data->lowPassRatio = params.getParam("low_pass_ratio", this->data->lowPassRatio); +} + +void MagnetometerCompass::setLowPassRatio(const double ratio) +{ + this->data->lowPassRatio = ratio; +} + +cras::expected MagnetometerCompass::computeAzimuth( + const sensor_msgs::Imu& imu, const sensor_msgs::MagneticField& magUnbiased) +{ + Imu imuInBody; + + try + { + this->data->tf->transform(imu, imuInBody, this->data->frame, ros::Duration(0.1)); + } + catch (const tf2::TransformException& e) + { + return cras::make_unexpected(cras::format( + "Could not transform IMU data to frame %s because: %s", this->data->frame.c_str(), e.what())); + } + + Field magUnbiasedInBody; + try + { + this->data->tf->transform(magUnbiased, magUnbiasedInBody, this->data->frame, ros::Duration(0.1)); + } + catch (const tf2::TransformException& e) + { + return cras::make_unexpected(cras::format( + "Could not transform magnetometer to frame %s because: %s", this->data->frame.c_str(), e.what())); + } + + // Compensate attitude in the magnetometer measurements + + double roll, pitch, yaw; + cras::getRPY(imuInBody.orientation, roll, pitch, yaw); + +#if 0 + tf2::Quaternion rot; + rot.setRPY(roll, pitch, 0); + tf2::Vector3 magNoAttitude; + tf2::convert(magUnbiasedInBody.magnetic_field, magNoAttitude); + magNoAttitude = tf2::quatRotate(rot, magNoAttitude); + + const auto magNorth = magNoAttitude.x(); + const auto magEast = magNoAttitude.y(); +#else + // Copied from INSO, not sure where do the numbers come from + const auto magNorth = + magUnbiasedInBody.magnetic_field.x * cos(pitch) + + magUnbiasedInBody.magnetic_field.y * sin(pitch) * sin(roll) + + magUnbiasedInBody.magnetic_field.z * sin(pitch) * cos(roll); + + const auto magEast = + magUnbiasedInBody.magnetic_field.y * cos(roll) - + magUnbiasedInBody.magnetic_field.z * sin(roll); +#endif + + // This formula gives north-referenced clockwise-increasing azimuth + const auto magAzimuthNow = atan2(magEast, magNorth); + tf2::Quaternion magAzimuthNowQuat; + magAzimuthNowQuat.setRPY(0, 0, magAzimuthNow); + + if (!this->data->lastAzimuth.has_value()) + this->data->lastAzimuth = magAzimuthNowQuat; + else // low-pass filter + this->data->lastAzimuth = this->data->lastAzimuth->slerp(magAzimuthNowQuat, 1 - this->data->lowPassRatio); + this->updateVariance(); + + compass_msgs::Azimuth nedAzimuthMsg; + nedAzimuthMsg.header.stamp = magUnbiased.header.stamp; + nedAzimuthMsg.header.frame_id = this->data->frame; + nedAzimuthMsg.azimuth = angles::normalize_angle_positive(cras::getYaw(*this->data->lastAzimuth)); + nedAzimuthMsg.variance = this->data->variance; + nedAzimuthMsg.unit = Az::UNIT_RAD; + nedAzimuthMsg.orientation = Az::ORIENTATION_NED; + nedAzimuthMsg.reference = Az::REFERENCE_MAGNETIC; + + return nedAzimuthMsg; +} + +void MagnetometerCompass::reset() +{ + this->data->variance = this->data->initialVariance; + this->data->lastAzimuth.reset(); +} + +void MagnetometerCompass::updateVariance() +{ + // TODO: measure consistency of IMU rotation and azimuth increase similar to + // https://www.sciencedirect.com/science/article/pii/S2405959519302929 +} + +} diff --git a/magnetometer_compass/src/magnetometer_compass_nodelet.cpp b/magnetometer_compass/src/magnetometer_compass_nodelet.cpp deleted file mode 100644 index f95a7da..0000000 --- a/magnetometer_compass/src/magnetometer_compass_nodelet.cpp +++ /dev/null @@ -1,597 +0,0 @@ -/** - * \file - * \brief Compute various azimuth values based on a magnetometer, IMU orientation and possibly also GPS coordinates. - * \author Martin Pecka - * SPDX-License-Identifier: BSD-3-Clause - * SPDX-FileCopyrightText: Czech Technical University in Prague - */ - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#define WMM2010 "wmm2010" -#define WMM2015 "wmm2015v2" -#define WMM2020 "wmm2020" - -namespace magnetometer_compass -{ - -typedef message_filters::sync_policies::ApproximateTime SyncPolicy; - -struct AzimuthPublishersConfigForOrientation -{ - uint8_t orientation {}; - uint8_t reference {}; - - ros::Publisher quatPub; - ros::Publisher imuPub; - ros::Publisher posePub; - ros::Publisher radPub; - ros::Publisher degPub; - - bool publishQuat {false}; - bool publishImu {false}; - bool publishPose {false}; - bool publishRad {false}; - bool publishDeg {false}; - - bool publish {false}; - - geometry_msgs::QuaternionStamped quatMsg; - compass_msgs::Azimuth azimuthMsg; - sensor_msgs::Imu imuMsg; - geometry_msgs::PoseWithCovarianceStamped poseMsg; - - tf2::Matrix3x3 rotMatrix; - double roll {}, pitch {}, yaw {}; - - void init(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::string& frameId, const std::string& paramPrefix, - const std::string& topicPrefix, uint8_t reference, uint8_t orientation, - const std::string& referenceStr, const std::string& orientationStr); - - void publishAzimuths(const tf2::Quaternion& azimuth, double variance, const ros::Time& stamp, - const sensor_msgs::Imu& imuInBody); -}; - -struct AzimuthPublishersConfig -{ - uint8_t reference {}; - - AzimuthPublishersConfigForOrientation ned; - AzimuthPublishersConfigForOrientation enu; - - bool publish {false}; - - const tf2::Quaternion nedToEnu {-M_SQRT2/2, -M_SQRT2/2, 0, 0}; - const tf2::Quaternion enuToNed {this->nedToEnu.inverse()}; - - void init(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::string& frameId, - const std::string& paramPrefix, const std::string& topicPrefix, uint8_t reference, const std::string& referenceStr); - - void publishAzimuths(const tf2::Quaternion& nedAzimuth, double variance, const ros::Time& stamp, - const sensor_msgs::Imu& imuInBody); -}; - -struct MagnetometerCompassNodeletPrivate -{ - std::map> magneticModels; - std::unique_ptr> imuSub; - std::unique_ptr> magSub; - std::unique_ptr> syncSub; - ros::Subscriber fixSub; - ros::Subscriber magBiasSub; - std::unique_ptr tfListener; - - ros::Publisher magUnbiasedPub; - bool publishMagUnbiased {false}; - - AzimuthPublishersConfig magPublishers; - AzimuthPublishersConfig truePublishers; - AzimuthPublishersConfig utmPublishers; -}; - -MagnetometerCompassNodelet::MagnetometerCompassNodelet() : data(new MagnetometerCompassNodeletPrivate) -{ -} - -MagnetometerCompassNodelet::~MagnetometerCompassNodelet() -{ -} - -void MagnetometerCompassNodelet::onInit() -{ - auto nh = this->getNodeHandle(); - auto pnh = this->getPrivateNodeHandle(); - - this->frame = pnh.param("frame", std::string("base_link")); - this->magAzimuthLowPass = pnh.param("low_pass_ratio", this->magAzimuthLowPass); - if (pnh.hasParam("magnetic_declination")) - { - const auto declination = pnh.param("magnetic_declination", 0.0); - this->lastMagneticDeclination.setRPY(0, 0, declination); - this->magneticDeclinationForced = true; - } - else - { - this->magneticModelsPath = pnh.param( - "magnetic_models_path", ros::package::getPath("magnetometer_compass") + "/data/magnetic"); - this->forcedMagneticModelName = pnh.param("magnetic_model", std::string()); - } - - this->variance = pnh.param("initial_variance", this->variance); - - if (pnh.hasParam("initial_mag_bias_x") || pnh.hasParam("initial_mag_bias_y") || pnh.hasParam("initial_mag_bias_z")) - { - sensor_msgs::MagneticField msg; - msg.magnetic_field.x = pnh.param("initial_mag_bias_x", this->lastMagBias.x()); - msg.magnetic_field.y = pnh.param("initial_mag_bias_y", this->lastMagBias.y()); - msg.magnetic_field.z = pnh.param("initial_mag_bias_z", this->lastMagBias.z()); - - const auto scalingMatrix = pnh.param("initial_mag_scaling_matrix", std::vector({})); - if (scalingMatrix.size() == 9) - std::copy(scalingMatrix.begin(), scalingMatrix.end(), msg.magnetic_field_covariance.begin()); - else if (!scalingMatrix.empty()) - ROS_ERROR("Parameter initial_mag_scaling_matrix has to have either 0 or 9 values."); - - Eigen::Map > covMatrix(msg.magnetic_field_covariance.data()); - const auto hasScale = covMatrix.cwiseAbs().sum() > 1e-10; - - ROS_INFO("Initial magnetometer bias is %0.3f %0.3f %0.3f %s scaling factor", - msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z, hasScale ? "with" : "without"); - - this->magBiasCb(msg); - } - - if (pnh.hasParam("initial_lat") && pnh.hasParam("initial_lon")) - { - sensor_msgs::NavSatFix msg; - if (pnh.hasParam("initial_year") && pnh.param("initial_year", 0) > 1970) - { - struct tm time{}; - time.tm_year = pnh.param("initial_year", 0); - msg.header.stamp.sec = static_cast(mktime(&time)); - } - else - { - // We can encounter invalid time when running a bag file and starting this node before the playback node - if (ros::Time::isValid()) - msg.header.stamp = ros::Time::now(); - else - msg.header.stamp.sec = ros::WallTime::now().sec; - } - - msg.latitude = pnh.param("initial_lat", this->lastFix.latitude); - msg.longitude = pnh.param("initial_lon", this->lastFix.longitude); - msg.altitude = pnh.param("initial_alt", this->lastFix.altitude); - - auto computedValues = "declination and grid convergence"; - if (this->magneticDeclinationForced) - computedValues = "grid convergence"; - ROS_INFO( - "Initial GPS coords for computation of %s are %.6f, %.6f, altitude %.0f m, year %u", - computedValues, msg.latitude, msg.longitude, msg.altitude, getYear(msg.header.stamp)); - - this->fixCb(msg); - } - - this->data->publishMagUnbiased = pnh.param("publish_mag_unbiased", this->data->publishMagUnbiased); - - // Set default publishers - this->data->magPublishers.ned.publishDeg = true; - - bool publish = this->data->publishMagUnbiased; - - // Read the publish_* parameters and set up the selected publishers - this->data->magPublishers.init(nh, pnh, this->frame, "publish", "compass", - compass_msgs::Azimuth::REFERENCE_MAGNETIC, "mag"); - publish |= this->data->magPublishers.publish; - this->data->truePublishers.init(nh, pnh, this->frame, "publish", "compass", - compass_msgs::Azimuth::REFERENCE_GEOGRAPHIC, "true"); - publish |= this->data->truePublishers.publish; - this->data->utmPublishers.init(nh, pnh, this->frame, "publish", "compass", - compass_msgs::Azimuth::REFERENCE_UTM, "utm"); - publish |= this->data->utmPublishers.publish; - - if (!publish) - ROS_WARN("No publishers have been requested. Please, set one of the publish_* parameters to true."); - - if (this->data->publishMagUnbiased) - this->data->magUnbiasedPub = nh.advertise("imu/mag_unbiased", 10); - - this->data->tfListener = std::make_unique(this->tf, nh); - - this->data->imuSub = std::make_unique>(nh, "imu/data", 100); - this->data->magSub = std::make_unique>(nh, "imu/mag", 100); - this->data->syncSub = std::make_unique>(SyncPolicy(200), - *this->data->imuSub, *this->data->magSub); - this->data->syncSub->registerCallback(&MagnetometerCompassNodelet::imuMagCb, this); - - this->data->fixSub = nh.subscribe("gps/fix", 10, &MagnetometerCompassNodelet::fixCb, this); - this->data->magBiasSub = nh.subscribe("imu/mag_bias", 10, &MagnetometerCompassNodelet::magBiasCb, this); -} - -void AzimuthPublishersConfigForOrientation::init(ros::NodeHandle& nh, ros::NodeHandle& pnh, - const std::string& frameId, const std::string& paramPrefix, const std::string& topicPrefix, - const uint8_t reference, const uint8_t orientation, - const std::string& referenceStr, const std::string& orientationStr) -{ - this->reference = reference; - this->orientation = orientation; - - this->quatMsg.header.frame_id = frameId; - this->azimuthMsg.header.frame_id = frameId; - this->imuMsg.header.frame_id = frameId; - this->poseMsg.header.frame_id = frameId; - - this->azimuthMsg.orientation = this->orientation; - this->azimuthMsg.reference = this->reference; - - auto prefix = paramPrefix + "_" + referenceStr + "_azimuth_" + orientationStr + "_"; - this->publishQuat = pnh.param(prefix + "quat", this->publishQuat); - this->publishImu = pnh.param(prefix + "imu", this->publishImu); - this->publishPose = pnh.param(prefix + "pose", this->publishPose); - this->publishRad = pnh.param(prefix + "rad", this->publishRad); - this->publishDeg = pnh.param(prefix + "deg", this->publishDeg); - - this->publish = this->publishQuat || this->publishImu || this->publishPose || this->publishDeg || this->publishRad; - - prefix = topicPrefix + "/" + referenceStr + "/" + orientationStr + "/"; - if (this->publishQuat) this->quatPub = nh.advertise(prefix + "quat", 10); - if (this->publishImu) this->imuPub = nh.advertise(prefix + "imu", 10); - if (this->publishPose) this->posePub = nh.advertise(prefix + "pose", 10); - if (this->publishRad) this->radPub = nh.advertise(prefix + "rad", 10); - if (this->publishDeg) this->degPub = nh.advertise(prefix + "deg", 10); -} - -void AzimuthPublishersConfig::init(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::string& frameId, - const std::string& paramPrefix, const std::string& topicPrefix, - const uint8_t reference, const std::string& referenceStr) -{ - this->reference = reference; - this->ned.init(nh, pnh, frameId, paramPrefix, topicPrefix, reference, - compass_msgs::Azimuth::ORIENTATION_NED, referenceStr, "ned"); - this->enu.init(nh, pnh, frameId, paramPrefix, topicPrefix, reference, - compass_msgs::Azimuth::ORIENTATION_ENU, referenceStr, "enu"); - this->publish = this->ned.publish || this->enu.publish; -} - -void MagnetometerCompassNodelet::imuMagCb(const sensor_msgs::Imu& imu, const sensor_msgs::MagneticField& mag) -{ - this->lastImu = imu; - this->lastMag = mag; - - if (!this->hasMagBias) - { - ROS_ERROR_DELAYED_THROTTLE(10.0, "Magnetometer bias not available, compass cannot work. Waiting..."); - return; - } - - try - { - auto transform = this->tf.lookupTransform(this->frame, imu.header.frame_id, imu.header.stamp, ros::Duration(0.1)); - tf2::doTransform(imu, this->lastImuInBody, transform); - } - catch (const tf2::TransformException& e) - { - NODELET_ERROR_THROTTLE(1.0, "Could not transform IMU data to frame %s because: %s", this->frame.c_str(), e.what()); - return; - } - - tf2::Vector3 field; - tf2::convert(mag.magnetic_field, field); - field = this->lastMagScale * (field - this->lastMagBias); - - sensor_msgs::MagneticField magUnbiased = mag; - tf2::convert(field, magUnbiased.magnetic_field); - - if (this->data->publishMagUnbiased) - this->data->magUnbiasedPub.publish(magUnbiased); - - try - { - this->lastMagUnbiasedInBody = this->tf.transform(magUnbiased, this->frame, ros::Duration(0.1)); - } - catch (const tf2::TransformException& e) - { - NODELET_ERROR_THROTTLE(1.0, "Could not transform magnetometer to frame %s because: %s", - this->frame.c_str(), e.what()); - return; - } - - // Compensate attitude in the magnetometer measurements - - tf2::Quaternion rot; - tf2::convert(this->lastImuInBody.orientation, rot); - tf2::Matrix3x3 rotMatrix(rot); - double roll, pitch, yaw; - rotMatrix.getRPY(roll, pitch, yaw); - -#if 0 - rot.setRPY(roll, pitch, 0); - tf2::Vector3 magNoAttitude; - tf2::convert(this->lastMagUnbiasedInBody.magnetic_field, magNoAttitude); - magNoAttitude = tf2::quatRotate(rot, magNoAttitude); - - const auto magNorth = magNoAttitude.x(); - const auto magEast = magNoAttitude.y(); -#else - // Copied from INSO, not sure where do the numbers come from - const auto magNorth = - this->lastMagUnbiasedInBody.magnetic_field.x * cos(pitch) + - this->lastMagUnbiasedInBody.magnetic_field.y * sin(pitch) * sin(roll) + - this->lastMagUnbiasedInBody.magnetic_field.z * sin(pitch) * cos(roll); - - const auto magEast = - this->lastMagUnbiasedInBody.magnetic_field.y * cos(roll) - - this->lastMagUnbiasedInBody.magnetic_field.z * sin(roll); -#endif - - // This formula gives north-referenced clockwise-increasing azimuth - const auto magAzimuthNow = atan2(magEast, magNorth); - tf2::Quaternion magAzimuthNowQuat; - magAzimuthNowQuat.setRPY(0, 0, magAzimuthNow); - - if (this->magAzimuth.w() == 0.0) - this->magAzimuth = magAzimuthNowQuat; - else // low-pass filter - this->magAzimuth = this->magAzimuth.slerp(magAzimuthNowQuat, 1 - this->magAzimuthLowPass); - this->updateVariance(); - - this->data->magPublishers.publishAzimuths(this->magAzimuth, this->variance, mag.header.stamp, this->lastImuInBody); - - if ((this->data->truePublishers.publish || this->data->utmPublishers.publish) && - this->lastMagneticDeclination.w() != 0.0) - { - const auto trueAzimuth = this->lastMagneticDeclination * this->magAzimuth; - this->data->truePublishers.publishAzimuths(trueAzimuth, this->variance, mag.header.stamp, this->lastImuInBody); - - if (this->data->utmPublishers.publish && this->lastFix.header.stamp != ros::Time(0)) - { - int zone; - bool isNorthHemisphere; - double northing, easting, utmGridConvergence, projectionScale; - GeographicLib::UTMUPS::Forward(this->lastFix.latitude, this->lastFix.longitude, - zone, isNorthHemisphere, easting, northing, utmGridConvergence, projectionScale); - utmGridConvergence = angles::from_degrees(utmGridConvergence); - - tf2::Quaternion utmGridConvergenceQuat; - utmGridConvergenceQuat.setRPY(0, 0, utmGridConvergence); - - const auto utmHeading = utmGridConvergenceQuat * trueAzimuth; - this->data->utmPublishers.publishAzimuths(utmHeading, this->variance, mag.header.stamp, this->lastImuInBody); - } - } -} - -void AzimuthPublishersConfig::publishAzimuths( - const tf2::Quaternion& nedAzimuth, const double variance, const ros::Time& stamp, const sensor_msgs::Imu& imuInBody) -{ - if (!this->publish) - return; - - if (this->ned.publish) - { - auto imuNed = imuInBody; // If IMU message should not be published, we fake it here with the ENU-referenced one - if (this->ned.publishImu) - { - geometry_msgs::TransformStamped tf; - tf.header.stamp = imuInBody.header.stamp; - tf.header.frame_id = imuInBody.header.frame_id + "_ned"; - tf2::convert(this->enuToNed, tf.transform.rotation); - tf2::doTransform(imuInBody, imuNed, tf); - } - this->ned.publishAzimuths(nedAzimuth, variance, stamp, imuNed); - } - - if (this->enu.publish) - { - // Rotate to ENU - auto enuAzimuth = this->nedToEnu * nedAzimuth; - - // Cancel out all non-yaw parts of the quaterion - this->enu.rotMatrix = static_cast(enuAzimuth); - this->enu.rotMatrix.getRPY(this->enu.roll, this->enu.pitch, this->enu.yaw); - enuAzimuth.setRPY(0, 0, this->enu.yaw); - - this->enu.publishAzimuths(enuAzimuth, variance, stamp, imuInBody); - } -} - -void AzimuthPublishersConfigForOrientation::publishAzimuths( - const tf2::Quaternion& azimuth, const double variance, const ros::Time& stamp, const sensor_msgs::Imu& imuInBody) -{ - if (this->publishQuat || this->publishImu || this->publishPose) - { - tf2::convert(azimuth, this->quatMsg.quaternion); - - if (this->publishQuat) - { - this->quatMsg.header.stamp = stamp; - this->quatPub.publish(this->quatMsg); - } - - if (this->publishImu) - { - // The IMU message comes in an arbitrarily-referenced frame, and we adjust its yaw to become georeferenced. - double azimuthYaw; - this->rotMatrix = static_cast(azimuth); - this->rotMatrix.getRPY(this->roll, this->pitch, azimuthYaw); - - tf2::Quaternion imuRot; - tf2::convert(imuInBody.orientation, imuRot); - this->rotMatrix = static_cast(imuRot); - this->rotMatrix.getRPY(this->roll, this->pitch, this->yaw); - - tf2::Quaternion desiredRot; - desiredRot.setRPY(this->roll, this->pitch, azimuthYaw); - - const auto diffRot = desiredRot * imuRot.inverse(); - - geometry_msgs::TransformStamped tf; - tf.header = imuInBody.header; - tf2::convert(diffRot, tf.transform.rotation); - tf2::doTransform(imuInBody, this->imuMsg, tf); - - this->imuMsg.orientation_covariance[8] = variance; - - this->imuPub.publish(this->imuMsg); - } - - if (this->publishPose) - { - this->poseMsg.header.stamp = stamp; - this->poseMsg.pose.pose.orientation = this->quatMsg.quaternion; - this->poseMsg.pose.covariance[35] = variance; - this->posePub.publish(this->poseMsg); - } - } - - if (this->publishDeg || this->publishRad) - { - this->azimuthMsg.header.stamp = stamp; - this->azimuthMsg.variance = variance; - - this->rotMatrix = static_cast(azimuth); - this->rotMatrix.getRPY(this->roll, this->pitch, this->yaw); - - const auto azimuthRad = angles::normalize_angle_positive(this->yaw); - if (this->publishRad) - { - this->azimuthMsg.azimuth = azimuthRad; - this->azimuthMsg.unit = compass_msgs::Azimuth::UNIT_RAD; - this->radPub.publish(this->azimuthMsg); - } - - if (this->publishDeg) - { - const auto azimuthDeg = angles::to_degrees(azimuthRad); - this->azimuthMsg.azimuth = azimuthDeg; - this->azimuthMsg.unit = compass_msgs::Azimuth::UNIT_DEG; - this->degPub.publish(this->azimuthMsg); - } - } -} - -std::string MagnetometerCompassNodelet::getBestMagneticModel(const ros::Time& date) const -{ - if (!this->forcedMagneticModelName.empty()) - return this->forcedMagneticModelName; - - const auto year = getYear(date); // If the conversion failed, year would be 0, thus triggering the last branch. - if (year >= 2020) - return WMM2020; - else if (year >= 2015) - return WMM2015; - else - return WMM2010; -} - -uint32_t MagnetometerCompassNodelet::getYear(const ros::Time& date) -{ - // Convert the given date to struct tm - const auto dateAsTimeT = static_cast(date.sec); - struct tm dateAsStructTm{}; - const auto result = gmtime_r(&dateAsTimeT, &dateAsStructTm); - - // This would be true if the time conversion failed (it should never happen, but...) - if (result == nullptr) - return 0; - - return dateAsStructTm.tm_year + 1900; -} - -double MagnetometerCompassNodelet::getMagneticDeclination(const sensor_msgs::NavSatFix& fix) const -{ - const auto year = getYear(fix.header.stamp); - if (this->data->magneticModels[year] == nullptr) - { - const auto model = this->getBestMagneticModel(fix.header.stamp); - - try - { - this->data->magneticModels[year] = - std::make_unique(model, this->magneticModelsPath); - } - catch (const GeographicLib::GeographicErr& e) - { - ROS_ERROR_THROTTLE(1.0, "Could not create magnetic field model %s for year %u because of the following error: %s", - model.c_str(), year, e.what()); - return 0.0; - } - - NODELET_INFO("Initialized magnetic model %s for year %u.", model.c_str(), year); - - const auto& magModel = *this->data->magneticModels[year]; - if (year < magModel.MinTime() || year > magModel.MaxTime()) - NODELET_ERROR("Using magnetic field model %s for an invalid year %u!", model.c_str(), year); - } - - const auto& magModel = *this->data->magneticModels[year]; - - if (fix.altitude < magModel.MinHeight() || fix.altitude > magModel.MaxHeight()) - { - NODELET_ERROR_THROTTLE(10.0, "Using magnetic field model %s in altitude %.0f m which is outside the model range.", - magModel.MagneticModelName().c_str(), fix.altitude); - } - - double Bx, By, Bz; - magModel(year, fix.latitude, fix.longitude, fix.altitude, Bx, By, Bz); - - double H, F, D, I; - GeographicLib::MagneticModel::FieldComponents(Bx, By, Bz, H, F, D, I); - - return angles::from_degrees(D); -} - -void MagnetometerCompassNodelet::fixCb(const sensor_msgs::NavSatFix& fix) -{ - this->lastFix = fix; - if (!this->magneticDeclinationForced) - this->lastMagneticDeclination.setRPY(0, 0, this->getMagneticDeclination(fix)); -} - -void MagnetometerCompassNodelet::magBiasCb(const sensor_msgs::MagneticField& bias) -{ - tf2::convert(bias.magnetic_field, this->lastMagBias); - Eigen::Map > covMatrix(bias.magnetic_field_covariance.data()); - if (covMatrix.cwiseAbs().sum() > 1e-10) - this->lastMagScale.setFromOpenGLSubMatrix(covMatrix.transpose().data()); - this->hasMagBias = true; -} - -void MagnetometerCompassNodelet::updateVariance() -{ - // TODO: measure consistency of IMU rotation and azimuth increase similar to - // https://www.sciencedirect.com/science/article/pii/S2405959519302929 -} - -} - -PLUGINLIB_EXPORT_CLASS(magnetometer_compass::MagnetometerCompassNodelet, nodelet::Nodelet) diff --git a/magnetometer_compass/test/test_magnetometer_compass.cpp b/magnetometer_compass/test/test_magnetometer_compass.cpp new file mode 100644 index 0000000..2097eac --- /dev/null +++ b/magnetometer_compass/test/test_magnetometer_compass.cpp @@ -0,0 +1,369 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for MagnetometerCompass. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; +using Imu = sensor_msgs::Imu; +using Field = sensor_msgs::MagneticField; + +TEST(MagnetometerCompass, ComputeAzimuth) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + magnetometer_compass::MagnetometerCompass compass(log, "base_link", tf); + compass.setLowPassRatio(0.0); + + ros::Time time(1664286802, 187375068); + + // First, publish imu + mag before tf, this should do nothing + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093 - -0.097227663; + mag.magnetic_field.y = -0.538677 - -0.692264333; + mag.magnetic_field.z = 0.157033 - 0; + + auto maybeAzimuth = compass.computeAzimuth(imu, mag); + + // Missing tf, nothing published + ASSERT_FALSE(maybeAzimuth.has_value()); + ASSERT_FALSE(maybeAzimuth.error().empty()); + + // Publish tf. Now it should have everything. + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + maybeAzimuth = compass.computeAzimuth(imu, mag); + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.534008 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(0.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // New data + time = {1664286802, 197458028}; + + imu.header.stamp = time; + imu.angular_velocity.x = -0.007707; + imu.angular_velocity.y = 0.003923; + imu.angular_velocity.z = 0.041115; + imu.linear_acceleration.x = -0.206485; + imu.linear_acceleration.y = -0.538767; + imu.linear_acceleration.z = -9.894861; + imu.orientation.x = 0.747334; + imu.orientation.y = -0.664305; + imu.orientation.z = 0.013382; + imu.orientation.w = -0.003285; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200 - -0.097227663; + mag.magnetic_field.y = -0.533960 - -0.692264333; + mag.magnetic_field.z = 0.149800 - 0; + + maybeAzimuth = compass.computeAzimuth(imu, mag); + + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.544417 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(0.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(MagnetometerCompass, ConfigFromParams) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + magnetometer_compass::MagnetometerCompass compass(log, "base_link", tf); + + XmlRpc::XmlRpcValue params; + params["low_pass_ratio"] = 0.0; + params["initial_variance"] = 4.0; + auto paramAdapter = std::make_shared(params, ""); + cras::BoundParamHelper paramHelper(log, paramAdapter); + + compass.configFromParams(paramHelper); + + ros::Time time(1664286802, 187375068); + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093 - -0.097227663; + mag.magnetic_field.y = -0.538677 - -0.692264333; + mag.magnetic_field.z = 0.157033 - 0; + + auto maybeAzimuth = compass.computeAzimuth(imu, mag); + + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.534008 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(4.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // New data + time = {1664286802, 197458028}; + + imu.header.stamp = time; + imu.angular_velocity.x = -0.007707; + imu.angular_velocity.y = 0.003923; + imu.angular_velocity.z = 0.041115; + imu.linear_acceleration.x = -0.206485; + imu.linear_acceleration.y = -0.538767; + imu.linear_acceleration.z = -9.894861; + imu.orientation.x = 0.747334; + imu.orientation.y = -0.664305; + imu.orientation.z = 0.013382; + imu.orientation.w = -0.003285; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200 - -0.097227663; + mag.magnetic_field.y = -0.533960 - -0.692264333; + mag.magnetic_field.z = 0.149800 - 0; + + maybeAzimuth = compass.computeAzimuth(imu, mag); + + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.544417 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(4.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +TEST(MagnetometerCompass, Reset) // NOLINT +{ + cras::LogHelperPtr log(new cras::MemoryLogHelper); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + magnetometer_compass::MagnetometerCompass compass(log, "base_link", tf); + compass.setLowPassRatio(0.5); + + ros::Time time(1664286802, 187375068); + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093 - -0.097227663; + mag.magnetic_field.y = -0.538677 - -0.692264333; + mag.magnetic_field.z = 0.157033 - 0; + + auto maybeAzimuth = compass.computeAzimuth(imu, mag); + + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.534008 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(0.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); + + // Without resetting the filter, the next azimuth would be a mean of the two. + compass.reset(); + + // New data + time = {1664286802, 197458028}; + + imu.header.stamp = time; + imu.angular_velocity.x = -0.007707; + imu.angular_velocity.y = 0.003923; + imu.angular_velocity.z = 0.041115; + imu.linear_acceleration.x = -0.206485; + imu.linear_acceleration.y = -0.538767; + imu.linear_acceleration.z = -9.894861; + imu.orientation.x = 0.747334; + imu.orientation.y = -0.664305; + imu.orientation.z = 0.013382; + imu.orientation.w = -0.003285; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200 - -0.097227663; + mag.magnetic_field.y = -0.533960 - -0.692264333; + mag.magnetic_field.z = 0.149800 - 0; + + maybeAzimuth = compass.computeAzimuth(imu, mag); + + ASSERT_TRUE(maybeAzimuth.has_value()); + + EXPECT_EQ(time, maybeAzimuth->header.stamp); + EXPECT_EQ("base_link", maybeAzimuth->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.544417 + 2 * M_PI, maybeAzimuth->azimuth, 1e-6); + EXPECT_EQ(0.0, maybeAzimuth->variance); + EXPECT_EQ(Az::UNIT_RAD, maybeAzimuth->unit); + EXPECT_EQ(Az::ORIENTATION_NED, maybeAzimuth->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, maybeAzimuth->reference); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::Time::init(); + + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_compass/test/test_magnetometer_compass_nodelet.cpp b/magnetometer_compass/test/test_magnetometer_compass_nodelet.cpp new file mode 100644 index 0000000..bf74341 --- /dev/null +++ b/magnetometer_compass/test/test_magnetometer_compass_nodelet.cpp @@ -0,0 +1,799 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for magnetometer_compass. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; +using Quat = geometry_msgs::QuaternionStamped; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Imu = sensor_msgs::Imu; +using Field = sensor_msgs::MagneticField; +using Fix = sensor_msgs::NavSatFix; + +ros::V_string my_argv; + +template +std::unique_ptr createNodelet(const cras::LogHelperPtr& log, + const ros::M_string& remaps = {}, + const std::shared_ptr& tf = nullptr) +{ + // Declaration order of these variables is important to make sure they can be properly stopped and destroyed. + auto nodelet = class_loader::impl::createInstance( + "magnetometer_compass::MagnetometerCompassNodelet", nullptr); + if (nodelet == nullptr) + return nullptr; + + { + const auto paramHelper = dynamic_cast(nodelet); + if (paramHelper != nullptr) + paramHelper->setLogger(log); + } + + const auto targetNodelet = dynamic_cast(nodelet); + if (targetNodelet == nullptr) + { + delete nodelet; + return nullptr; + } + + if (tf != nullptr) + targetNodelet->setBuffer(tf); + + nodelet->init(ros::this_node::getName(), remaps, my_argv, nullptr, nullptr); + + return std::unique_ptr(targetNodelet); +} + +double det(const boost::array& mat) +{ + return Eigen::Map>(mat.data()).determinant(); +} + +TEST(MagnetometerCompassNodelet, BasicConversion) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("publish_mag_azimuth_enu_rad", true); + pnh.setParam("publish_mag_azimuth_ned_deg", true); + pnh.setParam("publish_true_azimuth_enu_rad", true); + pnh.setParam("publish_true_azimuth_enu_imu", true); + pnh.setParam("publish_utm_azimuth_enu_rad", true); + pnh.setParam("publish_utm_azimuth_ned_quat", true); + pnh.setParam("publish_utm_azimuth_ned_pose", true); + pnh.setParam("publish_mag_unbiased", true); + pnh.setParam("low_pass_ratio", 0.0); // Disable smoothing + + std::map, cras::optional> az; + auto azCb = [&az](decltype(Az::unit) unit, decltype(Az::orientation) orientation, decltype(Az::reference) reference, + const Az::ConstPtr& msg) + { + az[std::make_tuple(unit, orientation, reference)] = *msg; + }; + + cras::optional lastImu; + auto imuCb = [&lastImu](const Imu::ConstPtr& msg) + { + lastImu = *msg; + }; + + cras::optional lastQuat; + auto quatCb = [&lastQuat](const Quat::ConstPtr& msg) + { + lastQuat = *msg; + }; + + cras::optional lastPose; + auto poseCb = [&lastPose](const Pose::ConstPtr& msg) + { + lastPose = *msg; + }; + + cras::optional lastField; + auto magCb = [&lastField](const Field::ConstPtr& msg) + { + lastField = *msg; + }; + + std::list pubs; + auto imuPub = nh.advertise("imu/data", 1); pubs.push_back(imuPub); + auto magPub = nh.advertise("imu/mag", 1); pubs.push_back(magPub); + auto magBiasPub = nh.advertise("imu/mag_bias", 1, true); pubs.push_back(magBiasPub); + auto fixPub = nh.advertise("gps/fix", 1, true); pubs.push_back(fixPub); + + std::list subs; + size_t numAzimuths {0u}; + auto magUnbiasedSub = nh.subscribe("imu/mag_unbiased", 1, magCb); subs.push_back(magUnbiasedSub); + auto azMagEnuRadSub = nh.subscribe("compass/mag/enu/rad", 1, + cras::bind_front(azCb, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC)); + subs.push_back(azMagEnuRadSub); numAzimuths++; + auto azMagNedDegSub = nh.subscribe("compass/mag/ned/deg", 1, + cras::bind_front(azCb, Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC)); + subs.push_back(azMagNedDegSub); numAzimuths++; + auto azTrueEnuRadSub = nh.subscribe("compass/true/enu/rad", 1, + cras::bind_front(azCb, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC)); + subs.push_back(azTrueEnuRadSub); numAzimuths++; + auto azUtmEnuRadSub = nh.subscribe("compass/utm/enu/rad", 1, + cras::bind_front(azCb, Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM)); + subs.push_back(azUtmEnuRadSub); numAzimuths++; + auto azTrueEnuImuSub = nh.subscribe("compass/true/enu/imu", 1, imuCb); subs.push_back(azTrueEnuImuSub); + auto azUtmNedQuatSub = nh.subscribe("compass/utm/ned/quat", 1, quatCb); subs.push_back(azUtmNedQuatSub); + auto azUtmNedPoseSub = nh.subscribe("compass/utm/ned/pose", 1, poseCb); subs.push_back(azUtmNedPoseSub); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + auto nodelet = createNodelet(log, {}, tf); + ASSERT_NE(nullptr, nodelet); + + const auto pubTest = [](const ros::Publisher& p) {return p.getNumSubscribers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(pubs.begin(), pubs.end(), pubTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for publisher connections."); + } + + const auto subTest = [](const ros::Subscriber& p) {return p.getNumPublishers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(subs.begin(), subs.end(), subTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for subscriber connections."); + } + + ASSERT_FALSE(std::any_of(pubs.begin(), pubs.end(), pubTest)); + ASSERT_FALSE(std::any_of(subs.begin(), subs.end(), subTest)); + + ros::Time time(1664286802, 187375068); + + // First, publish imu + mag before bias + fix + tf, this should do nothing + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + imuPub.publish(imu); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + magPub.publish(mag); + + for ( + size_t i = 0; + i < 5 && (!lastField || !lastImu || !lastQuat || !lastPose || az.size() < numAzimuths) + && ros::ok() && nodelet->ok(); + ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + // Missing bias + fix + tf, nothing published + ASSERT_FALSE(lastImu.has_value()); + ASSERT_FALSE(lastQuat.has_value()); + ASSERT_FALSE(lastPose.has_value()); + ASSERT_FALSE(lastField.has_value()); + ASSERT_TRUE(az.empty()); + + // Now, publish bias + fix, but not yet tf + + Field bias; + bias.header.stamp = time; + bias.header.frame_id = "imu"; + bias.magnetic_field.x = -0.097227663; + bias.magnetic_field.y = -0.692264333; + bias.magnetic_field.z = 0; + magBiasPub.publish(bias); + + Fix fix; + fix.header.stamp = time; + fix.header.frame_id = "gps"; + fix.latitude = 50.090806436; + fix.longitude = 14.133202857; + fix.altitude = 445.6146; + fix.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; + fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + fixPub.publish(fix); + + ros::spinOnce(); + + // Wait until the latched messages are received + ros::WallDuration(0.2).sleep(); + ros::spinOnce(); + + imuPub.publish(imu); + magPub.publish(mag); + + for ( + size_t i = 0; + i < 5 && (!lastField || !lastImu || !lastQuat || !lastPose || az.size() < numAzimuths) + && ros::ok() && nodelet->ok(); + ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + // Missing tf, nothing published except unbiased magnetometer + ASSERT_FALSE(lastImu.has_value()); + ASSERT_FALSE(lastQuat.has_value()); + ASSERT_FALSE(lastPose.has_value()); + ASSERT_TRUE(lastField.has_value()); + ASSERT_TRUE(az.empty()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.360320, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, lastField->magnetic_field.z, 1e-6); + + // Publish tf. Now it should have everything. + + lastField.reset(); + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + imuPub.publish(imu); + magPub.publish(mag); + + for ( + size_t i = 0; + i < 10 && (!lastField || !lastImu || !lastQuat || !lastPose || az.size() < numAzimuths) + && ros::ok() && nodelet->ok(); + ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastImu.has_value()); + ASSERT_TRUE(lastQuat.has_value()); + ASSERT_TRUE(lastPose.has_value()); + ASSERT_TRUE(lastField.has_value()); + ASSERT_EQ(numAzimuths, az.size()); + for (const auto& [key, a] : az) + ASSERT_TRUE(a.has_value()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.360320, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, lastField->magnetic_field.z, 1e-6); + + const auto radEnuMag = std::make_tuple(Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_MAGNETIC); + EXPECT_EQ(time, az[radEnuMag]->header.stamp); + EXPECT_EQ("base_link", az[radEnuMag]->header.frame_id); + EXPECT_NEAR(3.534008, az[radEnuMag]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuMag]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuMag]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuMag]->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, az[radEnuMag]->reference); + + const auto degNedMag = std::make_tuple(Az::UNIT_DEG, Az::ORIENTATION_NED, Az::REFERENCE_MAGNETIC); + EXPECT_EQ(time, az[degNedMag]->header.stamp); + EXPECT_EQ("base_link", az[degNedMag]->header.frame_id); + EXPECT_NEAR(90 - angles::to_degrees(3.534008) + 360, az[degNedMag]->azimuth, 1e-4); + EXPECT_EQ(0.0, az[degNedMag]->variance); + EXPECT_EQ(Az::UNIT_DEG, az[degNedMag]->unit); + EXPECT_EQ(Az::ORIENTATION_NED, az[degNedMag]->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, az[degNedMag]->reference); + + const auto radEnuTrue = std::make_tuple(Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_GEOGRAPHIC); + EXPECT_EQ(time, az[radEnuTrue]->header.stamp); + EXPECT_EQ("base_link", az[radEnuTrue]->header.frame_id); + EXPECT_NEAR(3.452292, az[radEnuTrue]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuTrue]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuTrue]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuTrue]->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, az[radEnuTrue]->reference); + + const auto radEnuUtm = std::make_tuple(Az::UNIT_RAD, Az::ORIENTATION_ENU, Az::REFERENCE_UTM); + EXPECT_EQ(time, az[radEnuUtm]->header.stamp); + EXPECT_EQ("base_link", az[radEnuUtm]->header.frame_id); + EXPECT_NEAR(3.440687, az[radEnuUtm]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuUtm]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuUtm]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuUtm]->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, az[radEnuUtm]->reference); + + EXPECT_EQ(time, lastImu->header.stamp); + EXPECT_EQ("base_link", lastImu->header.frame_id); + // The IMU comes out rotated to be georeferenced, so we can't directly compare the values, but their magnitude should + // remain the same. Determinants of the covariances should be the same, too. + tf2::Vector3 v1, v2; + tf2::fromMsg(imu.angular_velocity, v1); + tf2::fromMsg(lastImu->angular_velocity, v2); + EXPECT_NEAR(v1.length(), v2.length(), 1e-6); + tf2::fromMsg(imu.linear_acceleration, v1); + tf2::fromMsg(lastImu->linear_acceleration, v2); + EXPECT_NEAR(v1.length(), v2.length(), 1e-6); + Imu transImu; + tf->transform(imu, transImu, "base_link"); + EXPECT_NEAR(cras::getRoll(transImu.orientation), cras::getRoll(lastImu->orientation), 1e-4); + EXPECT_NEAR(cras::getPitch(transImu.orientation), cras::getPitch(lastImu->orientation), 1e-4); + EXPECT_NEAR(az[radEnuTrue]->azimuth, angles::normalize_angle_positive(cras::getYaw(lastImu->orientation)), 1e-4); + EXPECT_NEAR(det(imu.angular_velocity_covariance), det(lastImu->angular_velocity_covariance), 1e-6); + EXPECT_NEAR(det(imu.linear_acceleration_covariance), det(lastImu->linear_acceleration_covariance), 1e-6); + // We can't check orientation covariance, it could change + + EXPECT_EQ(time, lastQuat->header.stamp); + EXPECT_EQ("base_link", lastQuat->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.440687 + 2 * M_PI, angles::normalize_angle_positive(cras::getYaw(lastQuat->quaternion)), 1e-6); + + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.440687 + 2 * M_PI, + angles::normalize_angle_positive(cras::getYaw(lastPose->pose.pose.orientation)), 1e-6); + EXPECT_NEAR(0.0, lastPose->pose.covariance[5 * 6 + 5], 1e-6); + + // New data + + az.clear(); + lastImu.reset(); + lastQuat.reset(); + lastPose.reset(); + lastField.reset(); + time = {1664286802, 197458028}; + + imu.header.stamp = time; + imu.angular_velocity.x = -0.007707; + imu.angular_velocity.y = 0.003923; + imu.angular_velocity.z = 0.041115; + imu.linear_acceleration.x = -0.206485; + imu.linear_acceleration.y = -0.538767; + imu.linear_acceleration.z = -9.894861; + imu.orientation.x = 0.747334; + imu.orientation.y = -0.664305; + imu.orientation.z = 0.013382; + imu.orientation.w = -0.003285; + imuPub.publish(imu); + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200; + mag.magnetic_field.y = -0.533960; + mag.magnetic_field.z = 0.149800; + magPub.publish(mag); + + for (size_t i = 0; + i < 10 && (!lastField.has_value() || !lastImu.has_value() || az.size() < numAzimuths) && ros::ok() && nodelet->ok(); + ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastImu.has_value()); + ASSERT_TRUE(lastField.has_value()); + ASSERT_EQ(numAzimuths, az.size()); + for (const auto& [key, a] : az) + ASSERT_TRUE(a.has_value()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.361427, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, lastField->magnetic_field.z, 1e-6); + + EXPECT_EQ(time, az[radEnuMag]->header.stamp); + EXPECT_EQ("base_link", az[radEnuMag]->header.frame_id); + EXPECT_NEAR(3.544417, az[radEnuMag]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuMag]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuMag]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuMag]->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, az[radEnuMag]->reference); + + EXPECT_EQ(time, az[degNedMag]->header.stamp); + EXPECT_EQ("base_link", az[degNedMag]->header.frame_id); + EXPECT_NEAR(90 - angles::to_degrees(3.544417) + 360, az[degNedMag]->azimuth, 1e-4); + EXPECT_EQ(0.0, az[degNedMag]->variance); + EXPECT_EQ(Az::UNIT_DEG, az[degNedMag]->unit); + EXPECT_EQ(Az::ORIENTATION_NED, az[degNedMag]->orientation); + EXPECT_EQ(Az::REFERENCE_MAGNETIC, az[degNedMag]->reference); + + EXPECT_EQ(time, az[radEnuTrue]->header.stamp); + EXPECT_EQ("base_link", az[radEnuTrue]->header.frame_id); + EXPECT_NEAR(3.462701, az[radEnuTrue]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuTrue]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuTrue]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuTrue]->orientation); + EXPECT_EQ(Az::REFERENCE_GEOGRAPHIC, az[radEnuTrue]->reference); + + EXPECT_EQ(time, az[radEnuUtm]->header.stamp); + EXPECT_EQ("base_link", az[radEnuUtm]->header.frame_id); + EXPECT_NEAR(3.451096, az[radEnuUtm]->azimuth, 1e-6); + EXPECT_EQ(0.0, az[radEnuUtm]->variance); + EXPECT_EQ(Az::UNIT_RAD, az[radEnuUtm]->unit); + EXPECT_EQ(Az::ORIENTATION_ENU, az[radEnuUtm]->orientation); + EXPECT_EQ(Az::REFERENCE_UTM, az[radEnuUtm]->reference); + + EXPECT_EQ(time, lastImu->header.stamp); + EXPECT_EQ("base_link", lastImu->header.frame_id); + // The IMU comes out rotated to be georeferenced, so we can't directly compare the values, but their magnitude should + // remain the same. Determinants of the covariances should be the same, too. + tf2::fromMsg(imu.angular_velocity, v1); + tf2::fromMsg(lastImu->angular_velocity, v2); + EXPECT_NEAR(v1.length(), v2.length(), 1e-6); + tf2::fromMsg(imu.linear_acceleration, v1); + tf2::fromMsg(lastImu->linear_acceleration, v2); + EXPECT_NEAR(v1.length(), v2.length(), 1e-6); + tf->transform(imu, transImu, "base_link"); + EXPECT_NEAR(cras::getRoll(transImu.orientation), cras::getRoll(lastImu->orientation), 1e-4); + EXPECT_NEAR(cras::getPitch(transImu.orientation), cras::getPitch(lastImu->orientation), 1e-4); + EXPECT_NEAR(az[radEnuTrue]->azimuth, angles::normalize_angle_positive(cras::getYaw(lastImu->orientation)), 1e-4); + EXPECT_NEAR(det(imu.angular_velocity_covariance), det(lastImu->angular_velocity_covariance), 1e-6); + EXPECT_NEAR(det(imu.linear_acceleration_covariance), det(lastImu->linear_acceleration_covariance), 1e-6); + // We can't check orientation covariance, it could change + + EXPECT_EQ(time, lastQuat->header.stamp); + EXPECT_EQ("base_link", lastQuat->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.451096 + 2 * M_PI, angles::normalize_angle_positive(cras::getYaw(lastQuat->quaternion)), 1e-6); + + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.451096 + 2 * M_PI, + angles::normalize_angle_positive(cras::getYaw(lastPose->pose.pose.orientation)), 1e-6); + EXPECT_NEAR(0.0, lastPose->pose.covariance[5 * 6 + 5], 1e-6); +} + +TEST(MagnetometerCompassNodelet, InitFromParams) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("publish_utm_azimuth_ned_quat", true); + pnh.setParam("publish_mag_unbiased", true); + pnh.setParam("low_pass_ratio", 0.0); // Disable smoothing + pnh.setParam("initial_mag_bias_x", -0.097227663); + pnh.setParam("initial_mag_bias_y", -0.692264333); + pnh.setParam("initial_mag_bias_z", 0.0); + pnh.setParam("initial_lat", 50.090806436); + pnh.setParam("initial_lon", 14.133202857); + pnh.setParam("initial_alt", 445.6146); + + cras::optional lastQuat; + auto quatCb = [&lastQuat](const Quat::ConstPtr& msg) + { + lastQuat = *msg; + }; + + cras::optional lastField; + auto magCb = [&lastField](const Field::ConstPtr& msg) + { + lastField = *msg; + }; + + std::list pubs; + auto imuPub = nh.advertise("imu/data", 1); pubs.push_back(imuPub); + auto magPub = nh.advertise("imu/mag", 1); pubs.push_back(magPub); + + std::list subs; + size_t numAzimuths {0u}; + auto magUnbiasedSub = nh.subscribe("imu/mag_unbiased", 1, magCb); subs.push_back(magUnbiasedSub); + auto azUtmNedQuatSub = nh.subscribe("compass/utm/ned/quat", 1, quatCb); subs.push_back(azUtmNedQuatSub); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + auto nodelet = createNodelet(log, {}, tf); + ASSERT_NE(nullptr, nodelet); + + const auto pubTest = [](const ros::Publisher& p) {return p.getNumSubscribers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(pubs.begin(), pubs.end(), pubTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for publisher connections."); + } + + const auto subTest = [](const ros::Subscriber& p) {return p.getNumPublishers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(subs.begin(), subs.end(), subTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for subscriber connections."); + } + + ASSERT_FALSE(std::any_of(pubs.begin(), pubs.end(), pubTest)); + ASSERT_FALSE(std::any_of(subs.begin(), subs.end(), subTest)); + + ros::Time time(1664286802, 187375068); + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + // Publish imu + mag + tf without bias + fix, which should be substituted by params + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + imuPub.publish(imu); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + magPub.publish(mag); + + for (size_t i = 0; i < 10 && (!lastField || !lastQuat) && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastQuat.has_value()); + ASSERT_TRUE(lastField.has_value()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.360320, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, lastField->magnetic_field.z, 1e-6); + + EXPECT_EQ(time, lastQuat->header.stamp); + EXPECT_EQ("base_link", lastQuat->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.440687 + 2 * M_PI, angles::normalize_angle_positive(cras::getYaw(lastQuat->quaternion)), 1e-6); +} + +TEST(MagnetometerCompassNodelet, SubscribeMagUnbiased) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("publish_utm_azimuth_ned_quat", true); + pnh.setParam("subscribe_mag_unbiased", true); + pnh.setParam("low_pass_ratio", 0.0); // Disable smoothing + pnh.setParam("initial_lat", 50.090806436); + pnh.setParam("initial_lon", 14.133202857); + pnh.setParam("initial_alt", 445.6146); + + cras::optional lastQuat; + auto quatCb = [&lastQuat](const Quat::ConstPtr& msg) + { + lastQuat = *msg; + }; + + std::list pubs; + auto imuPub = nh.advertise("imu/data", 1); pubs.push_back(imuPub); + auto magPub = nh.advertise("imu/mag_unbiased", 1); pubs.push_back(magPub); + + std::list subs; + auto azUtmNedQuatSub = nh.subscribe("compass/utm/ned/quat", 1, quatCb); subs.push_back(azUtmNedQuatSub); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto tf = std::make_shared(); + tf->setUsingDedicatedThread(true); + + auto nodelet = createNodelet(log, {}, tf); + ASSERT_NE(nullptr, nodelet); + + const auto pubTest = [](const ros::Publisher& p) {return p.getNumSubscribers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(pubs.begin(), pubs.end(), pubTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for publisher connections."); + } + + const auto subTest = [](const ros::Subscriber& p) {return p.getNumPublishers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(subs.begin(), subs.end(), subTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for subscriber connections."); + } + + ASSERT_FALSE(std::any_of(pubs.begin(), pubs.end(), pubTest)); + ASSERT_FALSE(std::any_of(subs.begin(), subs.end(), subTest)); + + ros::Time time(1664286802, 187375068); + + geometry_msgs::TransformStamped baseLinkImuTf; + baseLinkImuTf.header.stamp = time; + baseLinkImuTf.header.frame_id = "base_link"; + baseLinkImuTf.child_frame_id = "imu"; + baseLinkImuTf.transform.translation.x = 0; + baseLinkImuTf.transform.translation.y = 0; + baseLinkImuTf.transform.translation.z = 0.15; + baseLinkImuTf.transform.rotation.x = 0.7071067811882787; + baseLinkImuTf.transform.rotation.y = -0.7071067811848163; + baseLinkImuTf.transform.rotation.z = 7.312301077167311e-14; + baseLinkImuTf.transform.rotation.w = -7.312301077203115e-14; + tf->setTransform(baseLinkImuTf, "test", true); + + // Publish imu + mag_unbiased + tf without fix, which should be substituted by params + + Imu imu; + imu.header.stamp = time; + imu.header.frame_id = "imu"; + imu.angular_velocity.x = -0.002507; + imu.angular_velocity.y = 0.015959; + imu.angular_velocity.z = 0.044427; + imu.linear_acceleration.x = 0.108412; + imu.linear_acceleration.y = 0.520543; + imu.linear_acceleration.z = -9.605243; + imu.orientation.x = 0.747476; + imu.orientation.y = -0.664147; + imu.orientation.z = 0.013337; + imu.orientation.w = -0.003273; + imu.angular_velocity_covariance[0 * 3 + 0] = 0.000436; + imu.angular_velocity_covariance[1 * 3 + 1] = 0.000436; + imu.angular_velocity_covariance[2 * 3 + 2] = 0.000436; + imu.linear_acceleration_covariance[0 * 3 + 0] = 0.0004; + imu.linear_acceleration_covariance[1 * 3 + 1] = 0.0004; + imu.linear_acceleration_covariance[2 * 3 + 2] = 0.0004; + imu.orientation_covariance[0 * 3 + 0] = 0.017453; + imu.orientation_covariance[1 * 3 + 1] = 0.017453; + imu.orientation_covariance[2 * 3 + 2] = 0.157080; + imuPub.publish(imu); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093 - -0.097227663; + mag.magnetic_field.y = -0.538677 - -0.692264333; + mag.magnetic_field.z = 0.157033 - 0.0; + magPub.publish(mag); + + for (size_t i = 0; i < 10 && (!lastQuat) && ros::ok() && nodelet->ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastQuat.has_value()); + + EXPECT_EQ(time, lastQuat->header.stamp); + EXPECT_EQ("base_link", lastQuat->header.frame_id); + EXPECT_NEAR(M_PI_2 - 3.440687 + 2 * M_PI, angles::normalize_angle_positive(cras::getYaw(lastQuat->quaternion)), 1e-6); +} + + +TEST(MagnetometerCompassNodelet, ThrowWhenSubPubBias) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + ros::NodeHandle nh, pnh("~"); + + pnh.deleteParam(""); + pnh.setParam("subscribe_mag_unbiased", true); + pnh.setParam("publish_mag_unbiased", true); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + EXPECT_THROW(createNodelet(log), std::runtime_error); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + // Remove the program name from argv because the nodelet handling code does not expect it + argc -= 1; + argv += 1; + ros::removeROSArgs(argc, argv, my_argv); + ros::init(argc, argv, "test_magnetometer_compass_nodelet"); + + ros::NodeHandle nh; // Just prevent ROS being uninited when the test-private nodehandles go out of scope + + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_compass/test/test_magnetometer_compass_nodelet.test b/magnetometer_compass/test/test_magnetometer_compass_nodelet.test new file mode 100644 index 0000000..90b39d3 --- /dev/null +++ b/magnetometer_compass/test/test_magnetometer_compass_nodelet.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/magnetometer_compass/test/test_visualize_azimuth_nodelet.cpp b/magnetometer_compass/test/test_visualize_azimuth_nodelet.cpp new file mode 100644 index 0000000..a3582d9 --- /dev/null +++ b/magnetometer_compass/test/test_visualize_azimuth_nodelet.cpp @@ -0,0 +1,201 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for azimuth visualization. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Az = compass_msgs::Azimuth; +using Quat = geometry_msgs::QuaternionStamped; +using Pose = geometry_msgs::PoseWithCovarianceStamped; +using Fix = sensor_msgs::NavSatFix; + +TEST(VisualizeAzimuthNodelet, Basic) // NOLINT +{ + ros::NodeHandle nh, pnh("~"); + + cras::optional lastPose; + auto poseCb = [&lastPose](const Pose::ConstPtr& msg) + { + lastPose = *msg; + }; + + std::list pubs; + auto azPub = nh.advertise("visualize_azimuth/azimuth", 1); pubs.push_back(azPub); + auto fixPub = nh.advertise("gps/fix", 1, true); pubs.push_back(fixPub); + + std::list subs; + auto visSub = nh.subscribe("visualize_azimuth/azimuth_vis", 1, poseCb); subs.push_back(visSub); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + const auto pubTest = [](const ros::Publisher& p) {return p.getNumSubscribers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(pubs.begin(), pubs.end(), pubTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for publisher connections."); + } + + const auto subTest = [](const ros::Subscriber& p) {return p.getNumPublishers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(subs.begin(), subs.end(), subTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for subscriber connections."); + } + + ASSERT_FALSE(std::any_of(pubs.begin(), pubs.end(), pubTest)); + ASSERT_FALSE(std::any_of(subs.begin(), subs.end(), subTest)); + + ros::Time time = cras::parseTime("2024-11-18T13:00:00Z"); + + Az azimuth; + azimuth.header.frame_id = "base_link"; + azimuth.header.stamp = time; + azimuth.azimuth = 0; + azimuth.variance = 0; + azimuth.unit = Az::UNIT_RAD; + azimuth.orientation = Az::ORIENTATION_ENU; + azimuth.reference = Az::REFERENCE_UTM; + azPub.publish(azimuth); + + for (size_t i = 0; i < 10 && !lastPose.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastPose.has_value()); + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_EQ(0, lastPose->pose.covariance[0]); + EXPECT_EQ(0, lastPose->pose.covariance[35]); + EXPECT_EQ(0, lastPose->pose.pose.position.x); + EXPECT_EQ(0, lastPose->pose.pose.position.y); + EXPECT_EQ(0, lastPose->pose.pose.position.z); + EXPECT_EQ(0, lastPose->pose.pose.orientation.x); + EXPECT_EQ(0, lastPose->pose.pose.orientation.y); + EXPECT_NEAR(M_SQRT1_2, lastPose->pose.pose.orientation.z, 1e-6); + EXPECT_NEAR(M_SQRT1_2, lastPose->pose.pose.orientation.w, 1e-6); + + lastPose.reset(); + azimuth.azimuth = M_PI_2; + azPub.publish(azimuth); + + for (size_t i = 0; i < 10 && !lastPose.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastPose.has_value()); + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_EQ(0, lastPose->pose.covariance[0]); + EXPECT_EQ(0, lastPose->pose.covariance[35]); + EXPECT_EQ(0, lastPose->pose.pose.position.x); + EXPECT_EQ(0, lastPose->pose.pose.position.y); + EXPECT_EQ(0, lastPose->pose.pose.position.z); + EXPECT_EQ(0, lastPose->pose.pose.orientation.x); + EXPECT_EQ(0, lastPose->pose.pose.orientation.y); + EXPECT_NEAR(0, lastPose->pose.pose.orientation.z, 1e-6); + EXPECT_NEAR(1, lastPose->pose.pose.orientation.w, 1e-6); + + lastPose.reset(); + azimuth.azimuth = M_PI; + azPub.publish(azimuth); + + for (size_t i = 0; i < 10 && !lastPose.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastPose.has_value()); + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_EQ(0, lastPose->pose.covariance[0]); + EXPECT_EQ(0, lastPose->pose.covariance[35]); + EXPECT_EQ(0, lastPose->pose.pose.position.x); + EXPECT_EQ(0, lastPose->pose.pose.position.y); + EXPECT_EQ(0, lastPose->pose.pose.position.z); + EXPECT_EQ(0, lastPose->pose.pose.orientation.x); + EXPECT_EQ(0, lastPose->pose.pose.orientation.y); + EXPECT_NEAR(-M_SQRT1_2, lastPose->pose.pose.orientation.z, 1e-6); + EXPECT_NEAR(M_SQRT1_2, lastPose->pose.pose.orientation.w, 1e-6); + + // We haven't yet supplied a fix, so change of reference is not possible + + lastPose.reset(); + azimuth.azimuth = M_PI; + azimuth.reference = Az::REFERENCE_MAGNETIC; + azPub.publish(azimuth); + + for (size_t i = 0; i < 5 && !lastPose.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_FALSE(lastPose.has_value()); + + Fix fix; + fix.header.stamp = time; + fix.header.frame_id = "gps"; + fix.latitude = 50.090806436; + fix.longitude = 14.133202857; + fix.altitude = 445.6146; + fix.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; + fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + fixPub.publish(fix); + + // Wait until the fix arrives + ros::WallDuration(0.2).sleep(); + + lastPose.reset(); + azimuth.azimuth = M_PI; + azPub.publish(azimuth); + + for (size_t i = 0; i < 10 && !lastPose.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastPose.has_value()); + EXPECT_EQ(time, lastPose->header.stamp); + EXPECT_EQ("base_link", lastPose->header.frame_id); + EXPECT_EQ(0, lastPose->pose.covariance[0]); + EXPECT_EQ(0, lastPose->pose.covariance[35]); + EXPECT_EQ(0, lastPose->pose.pose.position.x); + EXPECT_EQ(0, lastPose->pose.pose.position.y); + EXPECT_EQ(0, lastPose->pose.pose.position.z); + EXPECT_EQ(0, lastPose->pose.pose.orientation.x); + EXPECT_EQ(0, lastPose->pose.pose.orientation.y); + EXPECT_NEAR(-0.671109, lastPose->pose.pose.orientation.z, 1e-6); + EXPECT_NEAR(0.741358, lastPose->pose.pose.orientation.w, 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "test_visualize_azimuth_nodelet"); + ros::NodeHandle nh; // Just prevent ROS being uninited when the test-private nodehandles go out of scope + + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_compass/test/test_visualize_azimuth_nodelet.test b/magnetometer_compass/test/test_visualize_azimuth_nodelet.test new file mode 100644 index 0000000..2717341 --- /dev/null +++ b/magnetometer_compass/test/test_visualize_azimuth_nodelet.test @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/magnetometer_pipeline/CMakeLists.txt b/magnetometer_pipeline/CMakeLists.txt new file mode 100644 index 0000000..28f746e --- /dev/null +++ b/magnetometer_pipeline/CMakeLists.txt @@ -0,0 +1,97 @@ +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +cmake_minimum_required(VERSION 3.10.2) +project(magnetometer_pipeline) + +set(CMAKE_CXX_STANDARD 17) + +find_package(catkin REQUIRED COMPONENTS + cras_cpp_common + geometry_msgs + message_filters + nodelet + roscpp + sensor_msgs + tf2_eigen +) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES magnetometer_bias_remover magnetometer_bias_remover_filter + CATKIN_DEPENDS cras_cpp_common message_filters roscpp sensor_msgs +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(magnetometer_bias_remover src/bias_remover.cpp) +add_dependencies(magnetometer_bias_remover ${catkin_EXPORTED_TARGETS}) +target_link_libraries(magnetometer_bias_remover ${catkin_LIBRARIES} Eigen3::Eigen) + +add_library(magnetometer_bias_remover_filter src/message_filter.cpp) +add_dependencies(magnetometer_bias_remover_filter ${catkin_EXPORTED_TARGETS}) +target_link_libraries(magnetometer_bias_remover_filter ${catkin_LIBRARIES} magnetometer_bias_remover) + +add_library(magnetometer_bias_remover_nodelet nodelets/magnetometer_bias_remover_nodelet.cpp) +add_dependencies(magnetometer_bias_remover_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(magnetometer_bias_remover_nodelet ${catkin_LIBRARIES} magnetometer_bias_remover_filter) +cras_node_from_nodelet(magnetometer_bias_remover_nodelet + ${PROJECT_NAME}::MagnetometerBiasRemoverNodelet OUTPUT_NAME magnetometer_bias_remover) + +install(TARGETS magnetometer_bias_remover magnetometer_bias_remover_filter magnetometer_bias_remover_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +catkin_install_python(PROGRAMS nodes/magnetometer_bias_observer + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) + find_package(rostest REQUIRED) + find_package(roslaunch REQUIRED) + + roslint_custom(catkin_lint "-W2" .) + + # Roslint C++ - checks formatting and some other rules for C++ files + file(GLOB_RECURSE ROSLINT_FILES include/*.h src/*.cpp nodelets/*.cpp test/*.cpp) + set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ + -build/header_guard,-build/include,-readability/namespace,-whitespace/braces,-runtime/references,\ + -build/c++11,-readability/nolint,-readability/todo,-legal/copyright") + roslint_cpp(${ROSLINT_FILES}) + + # Run roslint on Python sources + file(GLOB_RECURSE PYTHON_FILES nodes/magnetometer_bias_observer) + roslint_python("${PYTHON_FILES}") + + roslint_add_test() + + roslaunch_add_file_check(launch/magnetometer_proc.launch) + + catkin_add_gtest(test_magnetometer_bias_remover test/test_bias_remover.cpp) + target_link_libraries(test_magnetometer_bias_remover magnetometer_bias_remover) + + catkin_add_gtest(test_magnetometer_bias_remover_filter test/test_message_filter.cpp) + target_link_libraries(test_magnetometer_bias_remover_filter magnetometer_bias_remover_filter) + + add_rostest_gtest(test_magnetometer_bias_remover_nodelet + test/test_bias_remover_nodelet.test test/test_bias_remover_nodelet.cpp) + target_link_libraries(test_magnetometer_bias_remover_nodelet ${catkin_LIBRARIES}) + roslaunch_add_file_check(test/test_bias_remover_nodelet.test USE_TEST_DEPENDENCIES) +endif() diff --git a/magnetometer_pipeline/README.md b/magnetometer_pipeline/README.md new file mode 100644 index 0000000..a7df18b --- /dev/null +++ b/magnetometer_pipeline/README.md @@ -0,0 +1,74 @@ + + + +# magnetometer\_pipeline + +Calibration and removing of magnetometer bias. + +## Node magnetometer\_bias\_remover and nodelet magnetometer\_pipeline/magnetometer\_compass\_remover + +For the magnetometer to work correctly, it is required to measure its bias. This node listens on the `imu/mag_bias` +topic for this measurement, and until at least one message arrives, the node will not publish anything. If you do not +have a node publishing the bias, you can alternatively provide it via parameters. Depending on the application, it +may be required to re-estimate the bias from time to time even during runtime. + +### Subscribed topics +- `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer measurements (bias not removed). +- `imu/mag_bias` (`sensor_msgs/MagneticField`): Bias of the magnetometer. This value will be subtracted from the + incoming magnetometer measurements. Messages on this topic do not + need to come repeatedly if the bias does not change. The + `magnetic_field_covariance` field can be "misused" to carry a 3x3 + bias scaling matrix. + +### Published topics (see above for explanation) +- `imu/mag_unbiased` (`sensor_msgs/MagneticField`): The magnetic field measurement with bias removed. + +If you want to remap all topics under a different namespace than `imu`, it is sufficient to remap just `imu:=new_imu`. + +### Parameters +- `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. +- `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. +- `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. +- `~initial_scaling_matrix` (double\[9\], optional): Magnetometer scaling matrix (row-major). +- If you specify any of the `~initial_mag_bias_*` params, the node does not need to receive the bias messages. + +## Node magnetometer\_bias\_observer + +Magnetometer bias estimation node. + +### Subscribed topics + +- `imu/mag` (`sensor_msgs/MagneticField`): The raw magnetometer measurements. + +### Published topics + +- `imu/mag_bias` (`sensor_msgs/MagneticField`): The estimated bias. +- `speak/warn` (`std_msgs/String`): Optional topic on which the node reports user instructions. + +### Provided services + +- `calibrate_magnetometer` (`std_srvs/Trigger`): Call this service to start bias estimation. Rotate the robot in as much + axes as possible during the calibration. + +### Parameters + +- `~measuring_time` (double, default 30 s): How long should the bias estimation phase be. +- `~2d_mode` (bool, default true): If true, the calibration expects motion in only 2 axes instead of 3. +- `~2d_mode_ignore_axis` ('X', 'Y' or 'Z', default autodetect): If you know which magnetometer local axis will not be + used in 2D calibration, you can set it here. +- `~load_from_params` (bool, default false): If true, initial bias estimate will be loaded from ROS params. +- `magnetometer_bias_x` (double, default 0.0): The initial bias estimate for X axis (if `~load_from_params` is true). +- `magnetometer_bias_y` (double, default 0.0): The initial bias estimate for Y axis (if `~load_from_params` is true). +- `magnetometer_bias_z` (double, default 0.0): The initial bias estimate for Z axis (if `~load_from_params` is true). +- `~load_from_file` (bool, default true): If true, the initial bias estimate will be loaded from + `~/.ros/magnetometer_calib.yaml`. +- `~calibration_file_path` (str, default `~/.ros/magnetometer_calib.yaml`): Path to the calibration file. +- `~save_to_file` (bool, default true): If true, the last estimated bias will be saved to the calibration file. + +## `MagnetometerBiasRemover` class + +Helper class to remove known bias from 3-axis magnetometer. + +## `BiasRemoverFilter` message filter + +Message filter providing magnetometer measurements with removed bias. \ No newline at end of file diff --git a/magnetometer_pipeline/include/magnetometer_pipeline/bias_remover.h b/magnetometer_pipeline/include/magnetometer_pipeline/bias_remover.h new file mode 100644 index 0000000..f30c5a6 --- /dev/null +++ b/magnetometer_pipeline/include/magnetometer_pipeline/bias_remover.h @@ -0,0 +1,73 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Remove known bias from 3-axis magnetometer. + * \author Martin Pecka + */ + +#include +#include + +#include +#include +#include +#include + +namespace magnetometer_pipeline +{ + +struct MagnetometerBiasRemoverPrivate; + +/** + * \brief Remove known bias from 3-axis magnetometer. + */ +class MagnetometerBiasRemover : public cras::HasLogger +{ +public: + explicit MagnetometerBiasRemover(const cras::LogHelperPtr& log); + virtual ~MagnetometerBiasRemover(); + + /** + * \brief Configure the bias remover from ROS parameters. + * \param[in] params The parameters. + * + * The following parameters are read: + * - `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. + * - `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. + * - `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. + * - `~initial_scaling_matrix` (double[9], optional): Magnetometer scaling matrix (row-major). + */ + void configFromParams(const cras::BoundParamHelper& params); + + /** + * \brief Whether bias has already been set. + * \return Whether bias has already been set. + */ + bool hasBias() const; + + /** + * \brief Whether bias has already been set and is different from identity. + * \return Whether bias has already been set. + */ + bool hasScale() const; + + /** + * \brief Callback for magnetometer bias. + * \param[in] bias The bias value. If `magnetic_field_covariance` has non-zero elements, it is interpreted + * as a row-wise scaling matrix. + */ + void setBias(const sensor_msgs::MagneticField& bias); + + /** + * \brief Callback for magnetometer measurements. + * \param[in] mag The raw measured magnetic field strength. + * \return The measured magnetic field corrected for bias, or error message. + */ + cras::expected removeBias(const sensor_msgs::MagneticField& mag); + +private: + std::unique_ptr data; //!< PIMPL +}; +} diff --git a/magnetometer_pipeline/include/magnetometer_pipeline/message_filter.h b/magnetometer_pipeline/include/magnetometer_pipeline/message_filter.h new file mode 100644 index 0000000..df85196 --- /dev/null +++ b/magnetometer_pipeline/include/magnetometer_pipeline/message_filter.h @@ -0,0 +1,102 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Message filter to remove bias from 3-axis magnetometer measurements. + * \author Martin Pecka + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace magnetometer_pipeline +{ + +/** + * \brief Message filter to remove bias from 3-axis magnetometer measurements. + * + * \sa https://wiki.ros.org/message_filters + * + * Example usage: + * \code{.cpp} + * ros::NodeHandle nh; + * message_filters::Subscriber magInput(nh, "imu/mag", 100); + * message_filters::Subscriber biasInput(nh, "imu/mag_bias", 10); + * magnetometer_pipeline::BiasRemoverFilter filter(log, magInput, biasInput); + * // filter->configFromParams(params); + * filter.registerCallback([](const sensor_msgs::MagneticField::ConstPtr& unbiasedMsg) { + * ... // Handle the unbiased data + * }); + * \endcode + */ +class BiasRemoverFilter : public message_filters::SimpleFilter, public cras::HasLogger +{ +public: + /** + * \brief Construct azimuth filter that can convert all parameters. + * + * \tparam MagInput The type of the input filter. + * \tparam BiasInput The type of the bias filter. + * \param[in] log Logger. + * \param[in] magInput The message filter producing raw magnetometer measurements messages. + * \param[in] biasInput The message filter producing magnetometer bias messages. + */ + template + BiasRemoverFilter(const cras::LogHelperPtr& log, MagInput& magInput, BiasInput& biasInput) : HasLogger(log) + { + this->remover = std::make_unique(log); + this->connectMagnetometerInput(magInput); + this->connectBiasInput(biasInput); + } + + virtual ~BiasRemoverFilter(); + + template + void connectMagnetometerInput(MagInput& f) + { + this->magConnection.disconnect(); + this->magConnection = f.registerCallback(&BiasRemoverFilter::cbMag, this); + } + + template + void connectBiasInput(BiasInput& f) + { + this->biasConnection.disconnect(); + this->biasConnection = f.registerCallback(&BiasRemoverFilter::cbBias, this); + // Bias can be a latched message, so we could miss the only message sent there. Resubscribe to be sure we get it. + f.subscribe(); + } + + /** + * \brief Configure the bias removal process from ROS parameters. + * \param[in] params The parameters. + * + * The following parameters are read: + * - `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. + * - `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. + * - `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. + * - `~initial_scaling_matrix` (double[9], optional): Magnetometer scaling matrix (row-major). + */ + virtual void configFromParams(const cras::BoundParamHelper& params); + +protected: + virtual void cbMag(const ros::MessageEvent& event); + virtual void cbBias(const ros::MessageEvent& event); + + message_filters::Connection magConnection; //!< Connection to the magnetometer measurements input. + message_filters::Connection biasConnection; //!< Connection to the bias input. + + std::unique_ptr remover; //!< The bias remover that does the actual computations. +}; + +} diff --git a/magnetometer_pipeline/launch/magnetometer_proc.launch b/magnetometer_pipeline/launch/magnetometer_proc.launch new file mode 100644 index 0000000..a45db21 --- /dev/null +++ b/magnetometer_pipeline/launch/magnetometer_proc.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/magnetometer_pipeline/nodelets.xml b/magnetometer_pipeline/nodelets.xml new file mode 100644 index 0000000..00eb78a --- /dev/null +++ b/magnetometer_pipeline/nodelets.xml @@ -0,0 +1,9 @@ + + + + + + Remove known bias from 3-axis magnetometer. + + diff --git a/magnetometer_pipeline/nodelets/magnetometer_bias_remover_nodelet.cpp b/magnetometer_pipeline/nodelets/magnetometer_bias_remover_nodelet.cpp new file mode 100644 index 0000000..a8dc82b --- /dev/null +++ b/magnetometer_pipeline/nodelets/magnetometer_bias_remover_nodelet.cpp @@ -0,0 +1,88 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Remove known bias from 3-axis magnetometer. + * \author Martin Pecka + */ + +#include + +#include +#include +#include +#include +#include +#include + +namespace magnetometer_pipeline +{ +using Field = sensor_msgs::MagneticField; + +/** + * \brief Remove known bias from 3-axis magnetometer. + * + * For the magnetometer to work correctly, it is required to measure its bias. This node listens on the `imu/mag_bias` + * topic for this measurement, and until at least one message arrives, the node will not publish anything. If you do not + * have a node publishing the bias, you can alternatively provide it via parameters. Depending on the application, it + * may be required to re-estimate the bias from time to time even during runtime. + * + * Subscribed topics: + * - `imu/mag` (`sensor_msgs/MagneticField`): 3-axis magnetometer measurements (bias not removed). + * - `imu/mag_bias` (`sensor_msgs/MagneticField`): Bias of the magnetometer. This value will be subtracted from the + * incoming magnetometer measurements. Messages on this topic do not + * need to come repeatedly if the bias does not change. The + * `magnetic_field_covariance` field can be "misused" to carry a 3x3 + * bias scaling matrix. + * + * Published topics (see above for explanation): + * - `imu/mag_unbiased` (`sensor_msgs/MagneticField`): The magnetic field measurement with bias removed. + * + * Parameters: + * - `~initial_mag_bias_x` (double, no default, optional): Magnetometer bias in the X axis. + * - `~initial_mag_bias_y` (double, no default, optional): Magnetometer bias in the Y axis. + * - `~initial_mag_bias_z` (double, no default, optional): Magnetometer bias in the Z axis. + * - `~initial_scaling_matrix` (double[9], optional): Magnetometer scaling matrix (row-major). + * - If you specify any of the `~initial_mag_bias_*` params, the node does not need to receive the bias messages. + */ +class MagnetometerBiasRemoverNodelet : public cras::Nodelet +{ +public: + MagnetometerBiasRemoverNodelet(); + ~MagnetometerBiasRemoverNodelet() override; + +protected: + void onInit() override; + + std::unique_ptr remover; //!< \brief The bias remover doing the actual work. + + std::unique_ptr> magSub; //!< \brief Subscriber for magnetic field measurements. + std::unique_ptr> magBiasSub; //!< \brief Subscriber for bias. + + ros::Publisher magUnbiasedPub; //!< \brief Publisher of unbiased measurements. +}; + +MagnetometerBiasRemoverNodelet::MagnetometerBiasRemoverNodelet() = default; + +MagnetometerBiasRemoverNodelet::~MagnetometerBiasRemoverNodelet() = default; + +void MagnetometerBiasRemoverNodelet::onInit() +{ + auto nh = this->getNodeHandle(); + auto topicNh = ros::NodeHandle(nh, "imu"); + auto params = this->privateParams(); + + this->magUnbiasedPub = topicNh.advertise("mag_unbiased", 10); + + this->magSub = std::make_unique>(topicNh, "mag", 100); + this->magBiasSub = std::make_unique>(topicNh, "mag_bias", 10); + + this->remover = std::make_unique(this->log, *this->magSub, *this->magBiasSub); + this->remover->configFromParams(*params); + this->remover->registerCallback([this](const Field::ConstPtr& msg) {this->magUnbiasedPub.publish(msg);}); +} + +} + +PLUGINLIB_EXPORT_CLASS(magnetometer_pipeline::MagnetometerBiasRemoverNodelet, nodelet::Nodelet) diff --git a/magnetometer_pipeline/nodes/magnetometer_bias_observer b/magnetometer_pipeline/nodes/magnetometer_bias_observer new file mode 100755 index 0000000..d0141dc --- /dev/null +++ b/magnetometer_pipeline/nodes/magnetometer_bias_observer @@ -0,0 +1,237 @@ +#!/usr/bin/env python + +# SPDX-License-Identifier: BSD-3-Clause +# SPDX-FileCopyrightText: Czech Technical University in Prague + +"""Magnetometer bias estimation node.""" + +import os +import time +import yaml + +import numpy as np + +import rospy +from std_msgs.msg import String +from sensor_msgs.msg import MagneticField +from std_srvs.srv import Trigger + + +class MagBiasObserver: + def __init__(self): + + self.x_min = 10000 + self.y_min = 10000 + self.z_min = 10000 + + self.x_max = -10000 + self.y_max = -10000 + self.z_max = -10000 + + self.x_mean = None + self.y_mean = None + self.z_mean = None + + self.last_mag = None + self.msg = MagneticField() + + self.frame_id = "imu" + self.started = False + + self.measuring_time = rospy.Duration(rospy.get_param('~measuring_time', 30)) + self.finish_measuring_time = rospy.Time(0) + self.two_d_mode = bool(rospy.get_param('~2d_mode', True)) # only calibrate in a plane + # If None, the axis will be autodetected; otherwise, specify either "X", "Y" or "Z" + self.ignore_axis = rospy.get_param('~2d_mode_ignore_axis', None) + + load_from_params = rospy.get_param("~load_from_params", False) + load_from_params = load_from_params and rospy.has_param("magnetometer_bias_x") + load_from_params = load_from_params and rospy.has_param("magnetometer_bias_y") + load_from_params = load_from_params and rospy.has_param("magnetometer_bias_z") + + default_file = os.path.join(os.environ.get("HOME", ""), ".ros", "magnetometer_calib.yaml") + self.calibration_file = rospy.get_param("~calibration_file_path", default_file) + + load_from_file = rospy.get_param("~load_from_file", True) + load_from_file = load_from_file and len(self.calibration_file) > 0 and os.path.exists(self.calibration_file) + + self.save_to_file = rospy.get_param("~save_to_file", True) + + self.pub = rospy.Publisher('imu/mag_bias', MagneticField, latch=True, queue_size=1) + self.speak_pub = rospy.Publisher('speak/warn', String, queue_size=1) + + self.sub = None # will be initialized in subscribe() + + self.calibrate_server = rospy.Service("calibrate_magnetometer", Trigger, self.calibrate_service_cb) + + if load_from_file: + with open(self.calibration_file, 'r') as f: + data = yaml.safe_load(f) + self.x_mean = float(data.get("magnetometer_bias_x", 0.0)) + self.y_mean = float(data.get("magnetometer_bias_y", 0.0)) + self.z_mean = float(data.get("magnetometer_bias_z", 0.0)) + time.sleep(0.01) # give the publishers some time to get set up + rospy.logwarn("Magnetometer calibration loaded from file {}.".format(self.calibration_file)) + self.pub_msg(allow_save=False) + elif load_from_params: + self.x_mean = float(rospy.get_param("magnetometer_bias_x")) + self.y_mean = float(rospy.get_param("magnetometer_bias_y")) + self.z_mean = float(rospy.get_param("magnetometer_bias_z")) + time.sleep(0.01) # give the publishers some time to get set up + rospy.logwarn("Magnetometer calibration loaded from parameters.") + self.pub_msg() + + def subscribe(self): + self.started = False + self.sub = rospy.Subscriber('imu/mag', MagneticField, self.mag_callback) + + def calibrate_service_cb(self, _): + if self.sub or rospy.Time.now() < self.finish_measuring_time: + rospy.logwarn("Calibration in progress, ignoring another request") + return False, "Calibration already running" + + self.subscribe() + return True, "Calibration started, rotate the robot for the following %i seconds" % (self.measuring_time.secs,) + + def speak(self, message): + msg = String() + msg.data = message + self.speak_pub.publish(msg) + + def mag_callback(self, msg): + self.frame_id = msg.header.frame_id + self.last_mag = msg + + if not self.started and rospy.Time.now().is_zero(): + return + + if not self.started: + self.started = True + self.finish_measuring_time = rospy.Time.now() + self.measuring_time + + log = "Started magnetometer calibration, rotate the robot several times in the following %i seconds." % \ + (self.measuring_time.secs,) + rospy.logwarn(log) + self.speak(log) + + if rospy.Time.now() < self.finish_measuring_time: + mag = msg.magnetic_field + + if mag.x > self.x_max: + self.x_max = mag.x + elif mag.x < self.x_min: + self.x_min = mag.x + + if mag.y > self.y_max: + self.y_max = mag.y + elif mag.y < self.y_min: + self.y_min = mag.y + + if mag.z > self.z_max: + self.z_max = mag.z + elif mag.z < self.z_min: + self.z_min = mag.z + else: + rospy.logwarn("Magnetometer calibrated") + self.speak("Magnetometer calibrated") + self.sub.unregister() # unsubscribe the mag messages + self.sub = None + + self.set_means() + self.pub_msg() + + def set_means(self): + self.x_mean = (self.x_min + self.x_max)/2 + self.y_mean = (self.y_min + self.y_max)/2 + self.z_mean = (self.z_min + self.z_max)/2 + + if self.two_d_mode: + x_range = self.x_max - self.x_min + y_range = self.y_max - self.y_min + z_range = self.z_max - self.z_min + + rospy.loginfo("range %f %f %f" % (x_range, y_range, z_range)) + + free_axis = None + if self.ignore_axis is not None: + free_axis = self.ignore_axis.upper() + elif x_range < min(0.75 * y_range, 0.75 * z_range): + free_axis = "X" + elif y_range < min(0.75 * x_range, 0.75 * z_range): + free_axis = "Y" + elif z_range < min(0.75 * x_range, 0.75 * y_range): + free_axis = "Z" + + if free_axis == "X": + self.x_mean = 0 + elif free_axis == "Y": + self.y_mean = 0 + elif free_axis == "Z": + self.z_mean = 0 + + if free_axis is not None: + rospy.loginfo("Magnetometer calibration finished in 2D mode with %s axis uncalibrated." % (free_axis,)) + else: + rospy.logwarn( + "Magnetometer calibration in 2D mode requested, but autodetection of the free axis failed. " + "Did you rotate the robot? Or did you do move with the robot in all 3 axes?") + + if self.last_mag is not None: + magnitude = np.linalg.norm([ + self.last_mag.magnetic_field.x - self.x_mean, + self.last_mag.magnetic_field.y - self.y_mean, + self.last_mag.magnetic_field.z - self.z_mean]) + logfn = rospy.loginfo if magnitude < 1e-4 else rospy.logwarn + logfn("Magnitude of the calibrated magnetic field is %f T." % (magnitude,)) + + def pub_msg(self, allow_save=True): + self.msg.header.stamp = rospy.Time.now() + self.msg.header.frame_id = self.frame_id + + self.msg.magnetic_field.x = self.x_mean + self.msg.magnetic_field.y = self.y_mean + self.msg.magnetic_field.z = self.z_mean + + self.pub.publish(self.msg) + + rospy.set_param("magnetometer_bias_x", self.x_mean) + rospy.set_param("magnetometer_bias_y", self.y_mean) + rospy.set_param("magnetometer_bias_z", self.z_mean) + + if self.save_to_file and allow_save: + dir = os.path.dirname(self.calibration_file) + if not os.path.exists(dir): + try: + os.makedirs(dir) + except OSError: + rospy.logerr("Could not create folder for storing calibration " + dir) + + if os.path.exists(dir): + data = { + "magnetometer_bias_x": self.x_mean, + "magnetometer_bias_y": self.y_mean, + "magnetometer_bias_z": self.z_mean, + } + try: + with open(self.calibration_file, 'w') as f: + yaml.safe_dump(data, f) + rospy.logwarn("Saved magnetometer calibration to file " + self.calibration_file) + except Exception as e: + rospy.logerr("An error occured while saving magnetometer calibration to file " + + self.calibration_file) + else: + rospy.logerr("Could not store magnetometer calibration to file {}. Cannot create the file.".format( + self.calibration_file)) + + def run(self): + rospy.spin() + + +def main(): + rospy.init_node('magnetometer_bias_observer') + node = MagBiasObserver() + node.run() + + +if __name__ == '__main__': + main() diff --git a/magnetometer_pipeline/package.xml b/magnetometer_pipeline/package.xml new file mode 100644 index 0000000..22b7191 --- /dev/null +++ b/magnetometer_pipeline/package.xml @@ -0,0 +1,48 @@ + + + + + magnetometer_pipeline + 1.0.3 + Calibration and removing of magnetometer bias. + + Martin Pecka + Martin Pecka + + BSD + + https://github.com/ctu-vras/compass + https://github.com/ctu-vras/compass/issues + https://wiki.ros.org/magnetometer_pipeline + + catkin + + cras_cpp_common + message_filters + roscpp + sensor_msgs + + eigen + geometry_msgs + nodelet + tf2_eigen + + geometry_msgs + nodelet + python-numpy + python3-numpy + rospy + std_msgs + std_srvs + tf2_eigen + + python-catkin-lint + python3-catkin-lint + roslaunch + roslint + rostest + + + + + diff --git a/magnetometer_pipeline/src/bias_remover.cpp b/magnetometer_pipeline/src/bias_remover.cpp new file mode 100644 index 0000000..ccef83e --- /dev/null +++ b/magnetometer_pipeline/src/bias_remover.cpp @@ -0,0 +1,105 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Remove known bias from 3-axis magnetometer. + * \author Martin Pecka + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace magnetometer_pipeline +{ +using Field = sensor_msgs::MagneticField; + +struct MagnetometerBiasRemoverPrivate +{ + //! \brief Whether magnetometer bias has already been established either from subscriber or initial parameters. + bool hasBias{false}; + + //! \brief Whether magnetometer scale has already been established either from subscriber or initial parameters and + //! it is different from identity. + bool hasScale{false}; + + //! \brief Last received value of magnetometer bias. + Eigen::Vector3d lastBias{0, 0, 0}; + + //! \brief Scaling factor of magnetometer measurements (optional). + Eigen::Matrix3d lastScale{Eigen::Matrix3d::Identity()}; +}; + +MagnetometerBiasRemover::MagnetometerBiasRemover(const cras::LogHelperPtr& log) : + cras::HasLogger(log), data(new MagnetometerBiasRemoverPrivate{}) +{ +} + +MagnetometerBiasRemover::~MagnetometerBiasRemover() = default; + +void MagnetometerBiasRemover::configFromParams(const cras::BoundParamHelper& params) +{ + if (params.hasParam("initial_mag_bias_x") || params.hasParam("initial_mag_bias_y") || + params.hasParam("initial_mag_bias_z")) + { + sensor_msgs::MagneticField msg; + msg.magnetic_field.x = params.getParam("initial_mag_bias_x", 0.0, "T"); + msg.magnetic_field.y = params.getParam("initial_mag_bias_y", 0.0, "T"); + msg.magnetic_field.z = params.getParam("initial_mag_bias_z", 0.0, "T"); + + Eigen::Map> scalingMatrix(msg.magnetic_field_covariance.data()); + scalingMatrix = params.getParam("initial_mag_scaling_matrix", this->data->lastScale); + + this->setBias(msg); + + CRAS_INFO("Initial magnetometer bias is %0.3f %0.3f %0.3f %s scaling factor", + msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z, this->hasScale() ? "with" : "without"); + } +} + +bool MagnetometerBiasRemover::hasBias() const +{ + return this->data->hasBias; +} + +bool MagnetometerBiasRemover::hasScale() const +{ + return this->data->hasScale; +} + +cras::expected MagnetometerBiasRemover::removeBias(const Field& mag) +{ + if (!this->data->hasBias) + return cras::make_unexpected("Magnetometer bias not available."); + + Eigen::Vector3d field; + tf2::fromMsg(mag.magnetic_field, field); + field = this->data->lastScale * (field - this->data->lastBias); + + Field magUnbiased = mag; + tf2::toMsg(field, magUnbiased.magnetic_field); + + return magUnbiased; +} + +void MagnetometerBiasRemover::setBias(const Field& bias) +{ + tf2::fromMsg(bias.magnetic_field, this->data->lastBias); + this->data->hasBias = true; + + Eigen::Map> covMatrix(bias.magnetic_field_covariance.data()); + if (covMatrix.cwiseAbs().sum() > 1e-10) + this->data->lastScale = covMatrix; + this->data->hasScale = (this->data->lastScale - Eigen::Matrix3d::Identity()).cwiseAbs().sum() > 1e-10; +} + +} diff --git a/magnetometer_pipeline/src/message_filter.cpp b/magnetometer_pipeline/src/message_filter.cpp new file mode 100644 index 0000000..ac40635 --- /dev/null +++ b/magnetometer_pipeline/src/message_filter.cpp @@ -0,0 +1,49 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Message filter to remove bias from 3-axis magnetometer measurements. + * \author Martin Pecka + */ + +#include + +#include +#include +#include +#include + +namespace magnetometer_pipeline +{ + +using Field = sensor_msgs::MagneticField; + +BiasRemoverFilter::~BiasRemoverFilter() = default; + +void BiasRemoverFilter::configFromParams(const cras::BoundParamHelper& params) +{ + this->remover->configFromParams(params); +} + +void BiasRemoverFilter::cbMag(const ros::MessageEvent& event) +{ + const auto maybeMagUnbiased = this->remover->removeBias(*event.getConstMessage()); + if (!maybeMagUnbiased.has_value()) + { + CRAS_ERROR_DELAYED_THROTTLE(10.0, "Bias remover cannot work: %s. Waiting...", maybeMagUnbiased.error().c_str()); + return; + } + + const auto header = event.getConnectionHeaderPtr(); + const auto stamp = event.getReceiptTime(); + this->signalMessage(ros::MessageEvent( + boost::make_shared(*maybeMagUnbiased), header, stamp, false, ros::DefaultMessageCreator())); +} + +void BiasRemoverFilter::cbBias(const ros::MessageEvent& event) +{ + this->remover->setBias(*event.getConstMessage()); +} + +} diff --git a/magnetometer_pipeline/test/test_bias_remover.cpp b/magnetometer_pipeline/test/test_bias_remover.cpp new file mode 100644 index 0000000..ffc3e73 --- /dev/null +++ b/magnetometer_pipeline/test/test_bias_remover.cpp @@ -0,0 +1,228 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for magnetometer_bias_remover. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using Field = sensor_msgs::MagneticField; + +TEST(MagnetometerBiasRemover, Basic) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto remover = magnetometer_pipeline::MagnetometerBiasRemover(log); + + EXPECT_FALSE(remover.hasBias()); + + ros::Time time(1664286802, 187375068); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + + EXPECT_FALSE(remover.hasBias()); + EXPECT_FALSE(remover.removeBias(mag).has_value()); + + // Set bias. Now it should have everything. + + Field bias; + bias.header.stamp = time; + bias.header.frame_id = "imu"; + bias.magnetic_field.x = -0.097227663; + bias.magnetic_field.y = -0.692264333; + bias.magnetic_field.z = 0; + remover.setBias(bias); + + EXPECT_TRUE(remover.hasBias()); + auto maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(0.360320, maybeMagUnbiased->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, maybeMagUnbiased->magnetic_field.z, 1e-6); + + // New data + + time = {1664286802, 197458028}; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200; + mag.magnetic_field.y = -0.533960; + mag.magnetic_field.z = 0.149800; + + maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(0.361427, maybeMagUnbiased->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, maybeMagUnbiased->magnetic_field.z, 1e-6); +} + +TEST(MagnetometerBiasRemover, ConfigFromParams) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto remover = magnetometer_pipeline::MagnetometerBiasRemover(log); + EXPECT_FALSE(remover.hasBias()); + + XmlRpc::XmlRpcValue config; + config.begin(); // set to dict type + config["initial_mag_bias_x"] = -0.097227663; + config["initial_mag_bias_y"] = -0.692264333; + config["initial_mag_bias_z"] = 0; + + cras::BoundParamHelper params(log, std::make_shared(config, "")); + remover.configFromParams(params); + + EXPECT_TRUE(remover.hasBias()); + + ros::Time time(1664286802, 187375068); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + + auto maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(0.360320, maybeMagUnbiased->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, maybeMagUnbiased->magnetic_field.z, 1e-6); + + // New data + + time = {1664286802, 197458028}; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200; + mag.magnetic_field.y = -0.533960; + mag.magnetic_field.z = 0.149800; + + maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(0.361427, maybeMagUnbiased->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, maybeMagUnbiased->magnetic_field.z, 1e-6); +} + +TEST(MagnetometerBiasRemover, ConfigFromParamsWithScale) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + auto remover = magnetometer_pipeline::MagnetometerBiasRemover(log); + EXPECT_FALSE(remover.hasBias()); + + XmlRpc::XmlRpcValue config; + config.begin(); // set to dict type + config["initial_mag_bias_x"] = -0.097227663; + config["initial_mag_bias_y"] = -0.692264333; + config["initial_mag_bias_z"] = 0; + config["initial_mag_scaling_matrix"][0 * 3 + 0] = 2.0; + config["initial_mag_scaling_matrix"][0 * 3 + 1] = 0.0; + config["initial_mag_scaling_matrix"][0 * 3 + 2] = 0.0; + config["initial_mag_scaling_matrix"][1 * 3 + 0] = 0.0; + config["initial_mag_scaling_matrix"][1 * 3 + 1] = 1.0; + config["initial_mag_scaling_matrix"][1 * 3 + 2] = 0.0; + config["initial_mag_scaling_matrix"][2 * 3 + 0] = 0.0; + config["initial_mag_scaling_matrix"][2 * 3 + 1] = 0.0; + config["initial_mag_scaling_matrix"][2 * 3 + 2] = 1.0; + + cras::BoundParamHelper params(log, std::make_shared(config, "")); + remover.configFromParams(params); + + EXPECT_TRUE(remover.hasBias()); + + ros::Time time(1664286802, 187375068); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + + auto maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(2 * 0.360320, maybeMagUnbiased->magnetic_field.x, 1e-5); + EXPECT_NEAR(0.153587, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, maybeMagUnbiased->magnetic_field.z, 1e-6); + + // New data + + time = {1664286802, 197458028}; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200; + mag.magnetic_field.y = -0.533960; + mag.magnetic_field.z = 0.149800; + + maybeMagUnbiased = remover.removeBias(mag); + ASSERT_TRUE(maybeMagUnbiased.has_value()); + + EXPECT_EQ(time, maybeMagUnbiased->header.stamp); + EXPECT_EQ("imu", maybeMagUnbiased->header.frame_id); + EXPECT_NEAR(2 * 0.361427, maybeMagUnbiased->magnetic_field.x, 1e-5); + EXPECT_NEAR(0.158304, maybeMagUnbiased->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, maybeMagUnbiased->magnetic_field.z, 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_pipeline/test/test_bias_remover_nodelet.cpp b/magnetometer_pipeline/test/test_bias_remover_nodelet.cpp new file mode 100644 index 0000000..035a957 --- /dev/null +++ b/magnetometer_pipeline/test/test_bias_remover_nodelet.cpp @@ -0,0 +1,155 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for magnetometer_bias_remover. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +using Field = sensor_msgs::MagneticField; + +TEST(MagnetometerBiasRemoverNodelet, Basic) // NOLINT +{ + // The values in this test are extracted from a real-world bag file recording. + + ros::NodeHandle nh, pnh("~"); + + cras::optional lastField; + auto magCb = [&lastField](const Field::ConstPtr& msg) + { + lastField = *msg; + }; + + std::list pubs; + auto magPub = nh.advertise("imu/mag", 1); pubs.push_back(magPub); + auto magBiasPub = nh.advertise("imu/mag_bias", 1, true); pubs.push_back(magBiasPub); + + std::list subs; + auto magUnbiasedSub = nh.subscribe("imu/mag_unbiased", 1, magCb); subs.push_back(magUnbiasedSub); + + // const auto log = std::make_shared(); + const auto log = std::make_shared(); + + const auto pubTest = [](const ros::Publisher& p) {return p.getNumSubscribers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(pubs.begin(), pubs.end(), pubTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for publisher connections."); + } + + const auto subTest = [](const ros::Subscriber& p) {return p.getNumPublishers() == 0;}; + for (size_t i = 0; i < 1000 && std::any_of(subs.begin(), subs.end(), subTest); ++i) + { + ros::WallDuration(0.01).sleep(); + ros::spinOnce(); + ROS_WARN_DELAYED_THROTTLE(0.2, "Waiting for subscriber connections."); + } + + ASSERT_FALSE(std::any_of(pubs.begin(), pubs.end(), pubTest)); + ASSERT_FALSE(std::any_of(subs.begin(), subs.end(), subTest)); + + ros::Time time(1664286802, 187375068); + + Field mag; + mag.header.stamp = time; + mag.header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.263093; + mag.magnetic_field.y = -0.538677; + mag.magnetic_field.z = 0.157033; + magPub.publish(mag); + + for (size_t i = 0; i < 5 && !lastField.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + // Missing bias, nothing published + ASSERT_FALSE(lastField.has_value()); + + // Publish bias. Now it should have everything. + + Field bias; + bias.header.stamp = time; + bias.header.frame_id = "imu"; + bias.magnetic_field.x = -0.097227663; + bias.magnetic_field.y = -0.692264333; + bias.magnetic_field.z = 0; + magBiasPub.publish(bias); + + ros::spinOnce(); + + // Wait until the latched messages are received + ros::WallDuration(0.2).sleep(); + ros::spinOnce(); + + magPub.publish(mag); + + for (size_t i = 0; i < 10 && !lastField.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastField.has_value()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.360320, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, lastField->magnetic_field.z, 1e-6); + + // New data + + lastField.reset(); + time = {1664286802, 197458028}; + + mag.header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag.magnetic_field.x = 0.264200; + mag.magnetic_field.y = -0.533960; + mag.magnetic_field.z = 0.149800; + magPub.publish(mag); + + for (size_t i = 0; i < 10 && !lastField.has_value() && ros::ok(); ++i) + { + ros::spinOnce(); + ros::WallDuration(0.1).sleep(); + } + ASSERT_TRUE(lastField.has_value()); + + EXPECT_EQ(time, lastField->header.stamp); + EXPECT_EQ("imu", lastField->header.frame_id); + EXPECT_NEAR(0.361427, lastField->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, lastField->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, lastField->magnetic_field.z, 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "test_magnetometer_bias_remover"); + ros::NodeHandle nh; // Just prevent ROS being uninited when the test-private nodehandles go out of scope + + return RUN_ALL_TESTS(); +} diff --git a/magnetometer_pipeline/test/test_bias_remover_nodelet.test b/magnetometer_pipeline/test/test_bias_remover_nodelet.test new file mode 100644 index 0000000..3ec4559 --- /dev/null +++ b/magnetometer_pipeline/test/test_bias_remover_nodelet.test @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/magnetometer_pipeline/test/test_message_filter.cpp b/magnetometer_pipeline/test/test_message_filter.cpp new file mode 100644 index 0000000..97b5b61 --- /dev/null +++ b/magnetometer_pipeline/test/test_message_filter.cpp @@ -0,0 +1,187 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief Unit test for magnetometer bias removing message filter. + * \author Martin Pecka + */ + +#include "gtest/gtest.h" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Field = sensor_msgs::MagneticField; + +template +class TestInput : public message_filters::SimpleFilter +{ +public: + void add(const typename T::ConstPtr& msg) + { + // Pass a complete MessageEvent to avoid calling ros::Time::now() to determine the missing timestamp + this->signalMessage(ros::MessageEvent(msg, msg->header.stamp)); + } + + void subscribe() + { + } +}; + +TEST(MessageFilter, Basic) // NOLINT +{ + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + TestInput magInput; + TestInput magBiasInput; + magnetometer_pipeline::BiasRemoverFilter filter(log, magInput, magBiasInput); + + Field::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + ros::Time time(1664286802, 187375068); + + Field::Ptr mag(new Field); + mag->header.stamp = time; + mag->header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag->magnetic_field.x = 0.263093; + mag->magnetic_field.y = -0.538677; + mag->magnetic_field.z = 0.157033; + + Field::Ptr bias(new Field); + bias->header.stamp = time; + bias->header.frame_id = "imu"; + bias->magnetic_field.x = -0.097227663; + bias->magnetic_field.y = -0.692264333; + bias->magnetic_field.z = 0; + + outMessage.reset(); + magInput.add(mag); + + EXPECT_EQ(nullptr, outMessage); + + outMessage.reset(); + magBiasInput.add(bias); + EXPECT_EQ(nullptr, outMessage); + + outMessage.reset(); + magInput.add(mag); + ASSERT_NE(nullptr, outMessage); + EXPECT_EQ(time, outMessage->header.stamp); + EXPECT_EQ("imu", outMessage->header.frame_id); + EXPECT_NEAR(0.360320, outMessage->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, outMessage->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, outMessage->magnetic_field.z, 1e-6); + + // New data + + time = {1664286802, 197458028}; + + mag->header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag->magnetic_field.x = 0.264200; + mag->magnetic_field.y = -0.533960; + mag->magnetic_field.z = 0.149800; + + outMessage.reset(); + magInput.add(mag); + ASSERT_NE(nullptr, outMessage); + EXPECT_EQ(time, outMessage->header.stamp); + EXPECT_EQ("imu", outMessage->header.frame_id); + EXPECT_NEAR(0.361427, outMessage->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, outMessage->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, outMessage->magnetic_field.z, 1e-6); +} + +TEST(MessageFilter, ConfigFromParams) // NOLINT +{ + const auto log = std::make_shared(); + // const auto log = std::make_shared(); + + TestInput magInput; + TestInput magBiasInput; + magnetometer_pipeline::BiasRemoverFilter filter(log, magInput, magBiasInput); + + Field::ConstPtr outMessage; + const auto cb = [&outMessage](const ros::MessageEvent& filteredMessage) + { + outMessage = filteredMessage.getConstMessage(); + }; + filter.registerCallback(boost::function&)>(cb)); + + XmlRpc::XmlRpcValue config; + config.begin(); // set to dict type + config["initial_mag_bias_x"] = -0.097227663; + config["initial_mag_bias_y"] = -0.692264333; + config["initial_mag_bias_z"] = 0; + + cras::BoundParamHelper params(log, std::make_shared(config, "")); + filter.configFromParams(params); + + ros::Time time(1664286802, 187375068); + + Field::Ptr mag(new Field); + mag->header.stamp = time; + mag->header.frame_id = "imu"; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag->magnetic_field.x = 0.263093; + mag->magnetic_field.y = -0.538677; + mag->magnetic_field.z = 0.157033; + + outMessage.reset(); + magInput.add(mag); + ASSERT_NE(nullptr, outMessage); + EXPECT_EQ(time, outMessage->header.stamp); + EXPECT_EQ("imu", outMessage->header.frame_id); + EXPECT_NEAR(0.360320, outMessage->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.153587, outMessage->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.157033, outMessage->magnetic_field.z, 1e-6); + + // New data + + time = {1664286802, 197458028}; + + mag->header.stamp = time; + // These values are exaggerated (in Gauss instead of in Tesla), but they're consistent with ethzasl_xsens_driver + // output. To just estimate the direction, it is no problem. + mag->magnetic_field.x = 0.264200; + mag->magnetic_field.y = -0.533960; + mag->magnetic_field.z = 0.149800; + + outMessage.reset(); + magInput.add(mag); + ASSERT_NE(nullptr, outMessage); + EXPECT_EQ(time, outMessage->header.stamp); + EXPECT_EQ("imu", outMessage->header.frame_id); + EXPECT_NEAR(0.361427, outMessage->magnetic_field.x, 1e-6); + EXPECT_NEAR(0.158304, outMessage->magnetic_field.y, 1e-6); + EXPECT_NEAR(0.149800, outMessage->magnetic_field.z, 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}