Restructure the repo, create compass_conversions, magnetic_model and magnetometer_pipeline packages.

.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

.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 }}

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.

README.md

@@ -0,0 +1,50 @@
+<!-- SPDX-License-Identifier: BSD-3-Clause -->
+<!-- SPDX-FileCopyrightText: Czech Technical University in Prague -->
+
+# 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 .

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)

compass_conversions/README.md

@@ -0,0 +1,138 @@
+<!-- SPDX-License-Identifier: BSD-3-Clause -->
+<!-- SPDX-FileCopyrightText: Czech Technical University in Prague -->
+
+# 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).