feat: make position in world to be the vessels

CMakeLists.txt

@@ -1,6 +1,7 @@
 PROJECT(nmea0183)
 cmake_minimum_required(VERSION 2.6)
 
+SET (CMAKE_CXX_STANDARD 17)
 SET (CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/.orogen/config")
 INCLUDE(nmea0183Base)
 

nmea0183.orogen

@@ -29,6 +29,11 @@ end
 
 # Parsing of an AIS NMEA0183 stream
 task_context "AISTask", subclasses: "nmea0183::MarnavTask" do
+    property "utm_configuration", "/gps_base/UTMConversionParameters"
+
+    # Whether to use the position correction or not
+    property "use_sensor_offset_correction", "/bool"
+
     output_port "positions", "/ais_base/Position"
     output_port "vessels_information", "/ais_base/VesselInformation"
     output_port "voyages_information", "/ais_base/VoyageInformation"

tasks/AISTask.cpp

@@ -36,73 +36,115 @@ bool AISTask::configureHook()
         return false;
     }
 
-    mAIS.reset(new AIS(*mDriver));
+    m_use_sensor_offset_correction = _use_sensor_offset_correction.get();
+    m_AIS.reset(new AIS(*mDriver));
     return true;
 }
 bool AISTask::startHook()
 {
-    if (! AISTaskBase::startHook()) {
+    if (!AISTaskBase::startHook()) {
         return false;
     }
+
+    m_UTM_converter.setParameters(_utm_configuration.get());
     return true;
 }
 void AISTask::updateHook()
 {
     AISTaskBase::updateHook();
 
-    mAISStats.time = base::Time::now();
-    mAISStats.discarded_sentences = mAIS->getDiscardedSentenceCount();
-    _ais_stats.write(mAISStats);
+    m_AIS_stats.time = base::Time::now();
+    m_AIS_stats.discarded_sentences = m_AIS->getDiscardedSentenceCount();
+    _ais_stats.write(m_AIS_stats);
 }
-bool AISTask::processSentence(marnav::nmea::sentence const& sentence) {
+bool AISTask::processSentence(marnav::nmea::sentence const& sentence)
+{
     if (sentence.id() != nmea::sentence_id::VDM) {
         return false;
     }
 
     unique_ptr<marnav::ais::message> msg;
     try {
-        msg = mAIS->processSentence(sentence);
+        msg = m_AIS->processSentence(sentence);
         if (!msg) {
             return true;
         }
     }
     catch (MarnavParsingError const& e) {
         LOG_ERROR_S << "error reported by marnav while creating an AIS message: "
-                    << e.what()
-                    << std::endl;
-        mAISStats.invalid_messages++;
+                    << e.what() << std::endl;
+        m_AIS_stats.invalid_messages++;
         return true;
     }
 
-    mAISStats.received_messages++;
+    m_AIS_stats.received_messages++;
     switch (msg->type()) {
         case ais::message_id::position_report_class_a: {
             auto msg01 = ais::message_cast<ais::message_01>(msg);
-            _positions.write(AIS::getPosition(*msg01));
+            auto position = AIS::getPosition(*msg01);
+            processPositionReport(position, position.mmsi);
             break;
         }
         case ais::message_id::position_report_class_a_assigned_schedule: {
             auto msg02 = ais::message_cast<ais::message_02>(msg);
-            _positions.write(AIS::getPosition(*msg02));
+            auto position = AIS::getPosition(*msg02);
+            processPositionReport(position, position.mmsi);
             break;
         }
         case ais::message_id::position_report_class_a_response_to_interrogation: {
             auto msg03 = ais::message_cast<ais::message_03>(msg);
-            _positions.write(AIS::getPosition(*msg03));
+            auto position = AIS::getPosition(*msg03);
+            processPositionReport(position, position.mmsi);
             break;
         }
         case ais::message_id::static_and_voyage_related_data: {
             auto msg05 = ais::message_cast<ais::message_05>(msg);
-            _vessels_information.write(AIS::getVesselInformation(*msg05));
-            _voyages_information.write(AIS::getVoyageInformation(*msg05));
+            auto vesselInformation = AIS::getVesselInformation(*msg05);
+            auto voyageInformation = AIS::getVoyageInformation(*msg05);
+            updateKnownVessels(vesselInformation);
+            _vessels_information.write(vesselInformation);
+            _voyages_information.write(voyageInformation);
             break;
         }
         default:
-            mAISStats.ignored_messages++;
+            m_AIS_stats.ignored_messages++;
             break;
     }
     return true;
 }
+void AISTask::processPositionReport(std::optional<ais_base::Position> position, int mmsi)
+{
+    if (!m_use_sensor_offset_correction) {
+        _positions.write(position.value());
+        return;
+    }
+
+    auto vessel = getCorrespondingVesselInfo(mmsi);
+    if (vessel.has_value()) {
+        auto corrected_position = AIS::applyPositionCorrection(position.value(),
+            vessel->reference_position,
+            m_UTM_converter);
+
+        position.value().latitude = corrected_position.latitude;
+        position.value().longitude = corrected_position.longitude;
+        position.value().correction_status = corrected_position.correction_status;
+    }
+    _positions.write(position.value());
+}
+std::optional<ais_base::VesselInformation> AISTask::getCorrespondingVesselInfo(int mmsi)
+{
+    if (m_vessels.find(mmsi) != m_vessels.end()) {
+        return m_vessels.at(mmsi);
+    }
+
+    return {};
+}
+void AISTask::updateKnownVessels(ais_base::VesselInformation info)
+{
+    if (m_vessels.find(info.mmsi) == m_vessels.end()) {
+        m_vessels[info.mmsi] = info;
+    }
+}
 void AISTask::errorHook()
 {
     AISTaskBase::errorHook();

tasks/AISTask.hpp

@@ -4,6 +4,10 @@
 #define NMEA0183_AISTASK_TASK_HPP
 
 #include "nmea0183/AISTaskBase.hpp"
+#include <base/samples/RigidBodyState.hpp>
+#include <gps_base/BaseTypes.hpp>
+#include <gps_base/UTMConverter.hpp>
+#include <optional>
 
 namespace nmea0183 {
     class Driver;
@@ -12,18 +16,23 @@ namespace nmea0183 {
     /*! \class AISTask
      * \brief Parsing of an AIS NMEA0183 stream
      */
-    class AISTask : public AISTaskBase
-    {
+    class AISTask : public AISTaskBase {
         /** The base class is auto-generated by orogen to define the task's interface
          *
          * It is located in the .orogen/tasks folder
          */
         friend class AISTaskBase;
 
     protected:
-        std::unique_ptr<AIS> mAIS;
-        AISStats mAISStats;
+        std::unique_ptr<AIS> m_AIS;
+        AISStats m_AIS_stats;
+        std::map<int, ais_base::VesselInformation> m_vessels;
+        gps_base::UTMConverter m_UTM_converter;
+        bool m_use_sensor_offset_correction;
+        void processPositionReport(std::optional<ais_base::Position> position, int mmsi);
         bool processSentence(marnav::nmea::sentence const& sentence);
+        std::optional<ais_base::VesselInformation> getCorrespondingVesselInfo(int mmsi);
+        void updateKnownVessels(ais_base::VesselInformation info);
 
     public:
         /** TaskContext constructor for AISTask