chore: move position correction code to lib

CMakeLists.txt

@@ -1,6 +1,7 @@
 # CMakeLists.txt has to be located in the project folder and cmake has to be
 # executed from 'project/build' with 'cmake ../'.
 cmake_minimum_required(VERSION 2.6)
+SET (CMAKE_CXX_STANDARD 17)
 find_package(Rock)
 rock_init(nmea0183 0.1)
 rock_standard_layout()

src/AIS.cpp

@@ -1,3 +1,4 @@
+#include <base-logging/Logging.hpp>
 #include <marnav/ais/ais.hpp>
 #include <marnav/nmea/vdm.hpp>
 #include <nmea0183/AIS.hpp>
@@ -8,6 +9,7 @@ using namespace marnav;
 using namespace nmea0183;
 
 double constexpr KNOTS_TO_MS = 0.514444;
+double constexpr MIN_SPEED_THRESHOLD = 0.2;
 
 AIS::AIS(Driver& driver)
     : mDriver(driver)
@@ -129,3 +131,107 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes
     info.destination = message.get_destination();
     return info;
 }
+
+std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> vesselToWorldOrientation(
+    const std::optional<base::Angle>& yaw,
+    const std::optional<base::Angle>& course_over_ground,
+    double speed_over_ground)
+{
+    if (yaw.has_value() && !std::isnan(yaw.value().getDeg())) {
+        return {Eigen::Quaterniond(
+                    Eigen::AngleAxisd(yaw.value().getRad(), Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING};
+    }
+    else if (course_over_ground.has_value() &&
+             !std::isnan(course_over_ground.value().getDeg()) &&
+             speed_over_ground >= MIN_SPEED_THRESHOLD) {
+        return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.value().getRad(),
+                    Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE};
+    }
+    else {
+        return {Eigen::Quaterniond::Identity(),
+            ais_base::PositionCorrectionStatus::POSITION_RAW};
+    }
+}
+
+base::Vector3d sensorToVesselInWorldPose(base::Vector3d sensor2vessel_pos,
+    Eigen::Quaterniond vessel2world_ori)
+{
+    base::Vector3d sensor2vessel_in_world_pos;
+    sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_pos;
+
+    return sensor2vessel_in_world_pos;
+}
+
+base::samples::RigidBodyState convertGPSToUTM(
+    const std::optional<ais_base::Position>& position,
+    gps_base::UTMConverter utm_converter)
+{
+    gps_base::Solution sensor2world_solution;
+    sensor2world_solution.latitude = position.value().latitude.getDeg();
+    sensor2world_solution.longitude = position.value().longitude.getDeg();
+
+    base::samples::RigidBodyState sensor2world_UTM;
+    sensor2world_UTM.position =
+        utm_converter.convertToUTM(sensor2world_solution).position;
+
+    return sensor2world_UTM;
+}
+
+std::pair<base::Angle, base::Angle> convertUTMToGPSInWorldFrame(
+    base::samples::RigidBodyState sensor2world_UTM,
+    base::Vector3d sensor2vessel_in_world_pos,
+    gps_base::UTMConverter utm_converter)
+{
+    base::samples::RigidBodyState vessel2world_UTM;
+    vessel2world_UTM.position = sensor2world_UTM.position + sensor2vessel_in_world_pos;
+
+    gps_base::Solution vessel2world_GPS;
+    vessel2world_GPS = utm_converter.convertUTMToGPS(vessel2world_UTM);
+
+    return {base::Angle::fromDeg(vessel2world_GPS.latitude),
+        base::Angle::fromDeg(vessel2world_GPS.longitude)};
+}
+
+ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position,
+    base::Vector3d const& vessel_reference_position,
+    gps_base::UTMConverter utm_converter)
+{
+    if (std::isnan(position.yaw.getDeg()) &&
+        std::isnan(position.course_over_ground.getDeg())) {
+        constexpr char error_msg[] = "Position can't be corrected because both 'yaw' "
+                                     "and 'course_over_ground' values are missing.";
+        LOG_ERROR_S << error_msg << std::endl;
+        throw std::runtime_error(error_msg);
+        return position;
+    }
+
+    auto [vessel2world_ori, status] = vesselToWorldOrientation(position.yaw,
+        position.course_over_ground,
+        position.speed_over_ground);
+
+    if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) {
+        constexpr char error_msg[] =
+            "Position can't be corrected because 'yaw' value is missing and "
+            "'speed_over_ground' is below the threshold.";
+        LOG_ERROR_S << error_msg << std::endl;
+        throw std::runtime_error(error_msg);
+        return position;
+    }
+
+    auto sensor2vessel_in_world_pos =
+        sensorToVesselInWorldPose(vessel_reference_position, vessel2world_ori);
+
+    auto [latitude, longitude] =
+        convertUTMToGPSInWorldFrame(convertGPSToUTM(position, utm_converter),
+            sensor2vessel_in_world_pos,
+            utm_converter);
+
+    ais_base::Position corrected_position;
+    corrected_position.latitude = latitude;
+    corrected_position.longitude = longitude;
+    corrected_position.correction_status = status;
+
+    return corrected_position;
+}

src/AIS.hpp

@@ -10,6 +10,11 @@
 #include <marnav/ais/message_01.hpp>
 #include <marnav/ais/message_05.hpp>
 
+#include <base/samples/RigidBodyState.hpp>
+#include <gps_base/BaseTypes.hpp>
+#include <gps_base/UTMConverter.hpp>
+#include <optional>
+
 namespace nmea0183 {
     class AIS {
         uint32_t mDiscardedSentenceCount = 0;
@@ -44,6 +49,23 @@ namespace nmea0183 {
          */
         uint32_t getDiscardedSentenceCount() const;
 
+        /**
+         * Applies position correction using the vessel reference position and the sensor
+         * offset
+         *
+         * @param position the AIS position to be corrected
+         * @param vessel_reference_position The position of the vessel’s reference point
+         * relative to the sensor
+         * @param utm_converter The UTM converter
+         *
+         * @return The corrected AIS position with updated latitude, longitude, and
+         * correction status
+         */
+        static ais_base::Position applyPositionCorrection(
+            ais_base::Position const& position,
+            base::Vector3d const& vessel_reference_position,
+            gps_base::UTMConverter utm_converter);
+
         static ais_base::Position getPosition(marnav::ais::message_01 const& message);
         static ais_base::VesselInformation getVesselInformation(
             marnav::ais::message_05 const& message);

test/test_AIS.cpp

@@ -25,6 +25,16 @@ struct AISTest : public ::testing::Test, public iodrivers_base::Fixture<Driver>
         uint8_t const* msg_u8 = reinterpret_cast<uint8_t const*>(msg.c_str());
         pushDataToDriver(msg_u8, msg_u8 + msg.size());
     }
+
+    gps_base::UTMConverter createUTMConverter()
+    {
+        gps_base::UTMConversionParameters parameters = {Eigen::Vector3d(0, 0, 0),
+            11,
+            true};
+        gps_base::UTMConverter converter(parameters);
+
+        return converter;
+    }
 };
 
 const std::vector<std::string> ais_strings = {
@@ -254,3 +264,88 @@ TEST_F(AISTest, it_converts_marnav_message05_into_a_VoyageInformation)
     ASSERT_EQ(7890, info.imo);
     ASSERT_EQ("DEST", info.destination);
 }
+
+TEST_F(AISTest, it_corrects_position_using_yaw)
+{
+    ais::message_01 msg;
+    msg.set_latitude(geo::latitude(45));
+    msg.set_longitude(geo::longitude(-120));
+    msg.set_sog(0);
+    msg.set_hdg(90);
+    auto position = AIS::getPosition(msg);
+
+    base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position =
+        AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
+
+    ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4);
+    ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4);
+    ASSERT_EQ(corrected_position.correction_status,
+        ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING);
+}
+
+TEST_F(AISTest, it_corrects_position_using_cog)
+{
+    ais::message_01 msg;
+    msg.set_latitude(geo::latitude(45));
+    msg.set_longitude(geo::longitude(-120));
+    msg.set_sog(0.5);
+    msg.set_cog(90);
+    auto position = AIS::getPosition(msg);
+
+    base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position =
+        AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
+
+    ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4);
+    ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4);
+    ASSERT_EQ(corrected_position.correction_status,
+        ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE);
+}
+
+TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing)
+{
+    ais::message_01 msg;
+    msg.set_latitude(geo::latitude(45));
+    msg.set_longitude(geo::longitude(-120));
+    msg.set_sog(0);
+    auto position = AIS::getPosition(msg);
+
+    base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    try {
+        AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
+    }
+    catch (const std::runtime_error& e) {
+        ASSERT_STREQ(e.what(),
+            "Position can't be corrected because both 'yaw' "
+            "and 'course_over_ground' values are missing.");
+    }
+}
+
+TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_threshold)
+{
+    ais::message_01 msg;
+    msg.set_latitude(geo::latitude(45));
+    msg.set_longitude(geo::longitude(-120));
+    msg.set_sog(0.1);
+    msg.set_cog(90);
+    auto position = AIS::getPosition(msg);
+
+    base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    try {
+        AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
+    }
+    catch (const std::runtime_error& e) {
+        ASSERT_STREQ(e.what(),
+            "Position can't be corrected because 'yaw' value is missing and "
+            "'speed_over_ground' is below the threshold.");
+    }
+}