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,16 +9,17 @@ using namespace marnav;
 using namespace nmea0183;
 
 double constexpr KNOTS_TO_MS = 0.514444;
+double constexpr MIN_SPEED_FOR_VALID_COURSE = 0.2;
 
 AIS::AIS(Driver& driver)
-    : mDriver(driver)
+    : m_driver(driver)
 {
 }
 
 unique_ptr<ais::message> AIS::readMessage()
 {
     while (true) {
-        auto sentence = mDriver.readSentence();
+        auto sentence = m_driver.readSentence();
         auto msg = processSentence(*sentence);
         if (msg) {
             return msg;
@@ -27,7 +29,7 @@ unique_ptr<ais::message> AIS::readMessage()
 
 uint32_t AIS::getDiscardedSentenceCount() const
 {
-    return mDiscardedSentenceCount;
+    return m_discarded_sentence_count;
 }
 
 unique_ptr<ais::message> AIS::processSentence(nmea::sentence const& sentence)
@@ -41,12 +43,12 @@ unique_ptr<ais::message> AIS::processSentence(nmea::sentence const& sentence)
     size_t n_fragments = vdm->get_n_fragments();
     size_t fragment = vdm->get_fragment();
     if (fragment != payloads.size() + 1) {
-        mDiscardedSentenceCount += payloads.size();
+        m_discarded_sentence_count += payloads.size();
         payloads.clear();
 
         // Go on if we're receiving the first fragment of a new message
         if (fragment != 1) {
-            mDiscardedSentenceCount++;
+            m_discarded_sentence_count++;
             return unique_ptr<ais::message>();
         }
     }
@@ -108,13 +110,18 @@ 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 distance_to_stern = message.get_to_stern();
+    auto distance_to_starboard = message.get_to_starboard();
+    float length = message.get_to_bow() + distance_to_stern;
+    float width = message.get_to_port() + distance_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);
+    info.reference_position = Eigen::Vector3d(distance_to_stern - (length / 2.0),
+        distance_to_starboard - (width / 2.0),
+        0);
 
     info.ensureEnumsValid();
     return info;
@@ -129,3 +136,101 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes
     info.destination = message.get_destination();
     return info;
 }
+
+std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> AIS::
+    selectVesselHeadingSource(base::Angle const& yaw,
+        base::Angle const& course_over_ground,
+        double speed_over_ground)
+{
+    if (!std::isnan(yaw.getRad())) {
+        return {
+            Eigen::Quaterniond(Eigen::AngleAxisd(yaw.getRad(), Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING};
+    }
+    else if (!std::isnan(course_over_ground.getRad()) &&
+             speed_over_ground >= MIN_SPEED_FOR_VALID_COURSE) {
+        return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.getRad(),
+                    Eigen::Vector3d::UnitZ())),
+            ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE};
+    }
+    else {
+        return {Eigen::Quaterniond::Identity(),
+            ais_base::PositionCorrectionStatus::POSITION_RAW};
+    }
+}
+
+base::Vector3d convertGPSToUTM(ais_base::Position const& position,
+    gps_base::UTMConverter const& utm_converter)
+{
+    gps_base::Solution sensor2world_solution;
+    sensor2world_solution.latitude = position.latitude.getDeg();
+    sensor2world_solution.longitude = position.longitude.getDeg();
+
+    auto sensor2world = utm_converter.convertToUTM(sensor2world_solution);
+
+    return sensor2world.position;
+}
+
+base::Vector3d computeVesselPositionInWorldFrame(
+    base::Vector3d const& sensor2vessel_pos,
+    Eigen::Quaterniond const& vessel2world_ori,
+    base::Vector3d const& sensor2world_pos)
+{
+    const auto sensor2world_ori = vessel2world_ori;
+    base::samples::RigidBodyState vessel2world;
+    vessel2world.position = sensor2world_pos - (sensor2world_ori * sensor2vessel_pos);
+    return vessel2world.position;
+}
+
+std::pair<base::Angle, base::Angle> convertUTMToGPS(
+    base::Vector3d const& vessel2world_pos,
+    gps_base::UTMConverter const& utm_converter)
+{
+    base::samples::RigidBodyState vessel2world;
+    vessel2world.position = vessel2world_pos;
+    gps_base::Solution vessel2world_gps;
+    vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world);
+
+    return {base::Angle::fromDeg(vessel2world_gps.latitude),
+        base::Angle::fromDeg(vessel2world_gps.longitude)};
+}
+
+ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& sensor_pos,
+    base::Vector3d const& sensor2vessel_pos,
+    gps_base::UTMConverter const& utm_converter)
+{
+    if (std::isnan(sensor_pos.yaw.getRad()) &&
+        std::isnan(sensor_pos.course_over_ground.getRad())) {
+        LOG_DEBUG_S << "Position can't be corrected because both 'yaw' "
+                       "and 'course_over_ground' values are missing."
+                    << std::endl;
+        return sensor_pos;
+    }
+
+    auto [vessel2world_ori, status] = selectVesselHeadingSource(sensor_pos.yaw,
+        sensor_pos.course_over_ground,
+        sensor_pos.speed_over_ground);
+
+    auto vessel_pos = sensor_pos;
+    if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) {
+        LOG_DEBUG_S << "Position can't be corrected because 'yaw' value is missing and "
+                       "'speed_over_ground' is below the threshold."
+                    << std::endl;
+        vessel_pos.correction_status = status;
+        return vessel_pos;
+    }
+
+    auto sensor2world_pos = convertGPSToUTM(sensor_pos, utm_converter);
+
+    auto vessel2world_pos = computeVesselPositionInWorldFrame(sensor2vessel_pos,
+        vessel2world_ori,
+        sensor2world_pos);
+
+    auto [latitude, longitude] = convertUTMToGPS(vessel2world_pos, utm_converter);
+
+    vessel_pos.latitude = latitude;
+    vessel_pos.longitude = longitude;
+    vessel_pos.correction_status = status;
+
+    return vessel_pos;
+}

src/AIS.hpp

@@ -10,10 +10,15 @@
 #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;
-        Driver& mDriver;
+        uint32_t m_discarded_sentence_count = 0;
+        Driver& m_driver;
         std::vector<std::pair<std::string, std::uint32_t>> payloads;
 
     public:
@@ -44,6 +49,43 @@ namespace nmea0183 {
          */
         uint32_t getDiscardedSentenceCount() const;
 
+        /**
+         * Applies position correction using the vessel reference position and the sensor
+         * offset
+         *
+         * @param position The vessel position to be corrected
+         * @param sensor2vessel_pos The position of the sensor relative to the
+         * vessel
+         * @param utm_converter The UTM converter
+         *
+         * @return The corrected vessel position with updated latitude, longitude, and
+         * correction status
+         */
+        static ais_base::Position applyPositionCorrection(
+            ais_base::Position const& position,
+            base::Vector3d const& sensor2vessel_pos,
+            gps_base::UTMConverter const& utm_converter);
+
+        /**
+         * @brief Selects the vessel's orientation in the world frame based on available
+         * heading or course information
+         * - Uses yaw if available
+         * - Uses course over ground if yaw is not available and the speed over ground is
+         * above the minimum threshold
+         * - Uses Identity otherwise
+         *
+         * @param yaw The vessel's yaw (heading) angle
+         * @param course_over_ground The vessel's course over ground angle
+         * @param speed_over_ground The vessel's speed over ground
+         *
+         * @return A pair containing the vessel's orientation and the position correction
+         * status
+         */
+        static std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus>
+        selectVesselHeadingSource(base::Angle const& yaw,
+            base::Angle const& course_over_ground,
+            double speed_over_ground);
+
         static ais_base::Position getPosition(marnav::ais::message_01 const& message);
         static ais_base::VesselInformation getVesselInformation(
             marnav::ais::message_05 const& message);