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)
@@ -108,13 +110,19 @@ ais_base::VesselInformation AIS::getVesselInformation(ais::message_05 const& mes
     string call_sign = message.get_callsign();
     first_not_space = call_sign.find_last_not_of(" ");
     info.call_sign = call_sign.substr(0, first_not_space + 1);
-    info.length = message.get_to_bow() + message.get_to_stern();
-    info.width = message.get_to_port() + message.get_to_starboard();
+    auto get_to_stern = message.get_to_stern();
+    auto get_to_starboard = message.get_to_starboard();
+    float length = message.get_to_bow() + get_to_stern;
+    float width = message.get_to_port() + get_to_starboard;
+    info.length = length;
+    info.width = width;
     info.draft = static_cast<float>(message.get_draught()) / 10;
     info.ship_type = static_cast<ais_base::ShipType>(message.get_shiptype());
     info.epfd_fix = static_cast<ais_base::EPFDFixType>(message.get_epfd_fix());
     info.reference_position =
-        Eigen::Vector3d(message.get_to_stern(), message.get_to_starboard(), 0);
+        Eigen::Vector3d(static_cast<double>(get_to_stern - length / 2.0),
+            static_cast<double>(get_to_starboard - width / 2.0),
+            0);
 
     info.ensureEnumsValid();
     return info;
@@ -129,3 +137,112 @@ 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().getRad())) {
+        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().getRad()) &&
+             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_in_vessel_pos,
+    Eigen::Quaterniond vessel2world_ori)
+{
+    base::Vector3d sensor2vessel_in_world_pos;
+    sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_in_vessel_pos;
+
+    return sensor2vessel_in_world_pos;
+}
+
+base::samples::RigidBodyState convertGPSToUTM(
+    const std::optional<ais_base::Position>& position,
+    gps_base::UTMConverter utm_converter)
+{
+    if (!position.has_value()) {
+        std::string error_msg = "Position data is unavailable";
+        LOG_ERROR_S << error_msg;
+        throw std::invalid_argument(error_msg);
+    }
+
+    auto position_value = position.value();
+    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;
+    sensor2world_UTM.time = base::Time::now();
+
+    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& sensor2vessel_in_vessel_pos,
+    gps_base::UTMConverter utm_converter)
+{
+    if (std::isnan(position.yaw.getRad()) &&
+        std::isnan(position.course_over_ground.getRad())) {
+        LOG_ERROR_S << "Position can't be corrected because both 'yaw' "
+                       "and 'course_over_ground' values are missing."
+                    << std::endl;
+        return position;
+    }
+
+    auto [vessel2world_ori, status] = vesselToWorldOrientation(position.yaw,
+        position.course_over_ground,
+        position.speed_over_ground);
+
+    if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) {
+        LOG_ERROR_S << "Position can't be corrected because 'yaw' value is missing and "
+                       "'speed_over_ground' is below the threshold."
+                    << std::endl;
+        return position;
+    }
+
+    auto sensor2vessel_in_world_pos =
+        sensorToVesselInWorldPose(sensor2vessel_in_vessel_pos, 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 sensor2vessel_in_vessel_pos The position of the sensor relative to the
+         * vessel
+         * @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& sensor2vessel_in_vessel_pos,
+            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(1, 1, 0),
+            11,
+            true};
+        gps_base::UTMConverter converter(parameters);
+
+        return converter;
+    }
 };
 
 const std::vector<std::string> ais_strings = {
@@ -180,7 +190,7 @@ TEST_F(AISTest, it_converts_marnav_message05_into_a_VesselInformation)
     ASSERT_EQ(ais_base::SHIP_TYPE_CARGO, info.ship_type);
     ASSERT_EQ(15, info.length);
     ASSERT_EQ(6, info.width);
-    ASSERT_EQ(base::Vector3d(10, 4, 0), info.reference_position);
+    ASSERT_EQ(base::Vector3d(2.5, 1, 0), info.reference_position);
     ASSERT_EQ(ais_base::EPFD_COMBINED_GPS_GLONASS, info.epfd_fix);
     ASSERT_NEAR(0.7, info.draft, 1e-2);
 }
@@ -254,3 +264,97 @@ 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 sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position = AIS::applyPositionCorrection(position,
+        sensor2vessel_in_vessel_pos,
+        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 sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position = AIS::applyPositionCorrection(position,
+        sensor2vessel_in_vessel_pos,
+        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 sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position = AIS::applyPositionCorrection(position,
+        sensor2vessel_in_vessel_pos,
+        utm_converter);
+    ASSERT_EQ(corrected_position.correction_status,
+        ais_base::PositionCorrectionStatus::POSITION_RAW);
+}
+
+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 sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    ais_base::Position corrected_position = AIS::applyPositionCorrection(position,
+        sensor2vessel_in_vessel_pos,
+        utm_converter);
+    ASSERT_EQ(corrected_position.correction_status,
+        ais_base::PositionCorrectionStatus::POSITION_RAW);
+}
+
+TEST_F(AISTest, it_throws_runtime_error_if_position_has_no_value)
+{
+    base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0);
+    gps_base::UTMConverter utm_converter = createUTMConverter();
+
+    try {
+        AIS::applyPositionCorrection({}, sensor2vessel_in_vessel_pos, utm_converter);
+    }
+    catch (const std::runtime_error& e) {
+        ASSERT_STREQ(e.what(), "Position data is unavailable.");
+    }
+}