From 87d41c105054ef71c778d3df504eed923c39b745 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 23 Jan 2025 09:00:21 -0300 Subject: [PATCH 01/20] chore: set cpp17 version in CMakeLists --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 42c5b5f..4b9b94b 100644 --- a/CMakeLists.txt +++ b/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() From ae7baa63fce14f3a6924293bde13086b14c2c59e Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 23 Jan 2025 09:01:32 -0300 Subject: [PATCH 02/20] chore: move position correction code to lib and make some improvements in the code. --- src/AIS.cpp | 87 +++++++++++++++++++++++++++++++++++++++++++++++++++++ src/AIS.hpp | 22 ++++++++++++++ 2 files changed, 109 insertions(+) diff --git a/src/AIS.cpp b/src/AIS.cpp index b8e0ed7..2e2bbe9 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -129,3 +129,90 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes info.destination = message.get_destination(); return info; } + +std::pair vesselToWorldOrientation( + const std::optional& yaw, + const std::optional& course_over_ground, + double speed_over_ground) +{ + const double min_speed_threshold = 0.2; + + if (yaw.has_value()) { + 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() && 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& 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 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) +{ + ais_base::Position corrected_position; + + auto [vessel2world_ori, status] = vesselToWorldOrientation(position.yaw, + position.course_over_ground, + position.speed_over_ground); + + 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); + + corrected_position.latitude = latitude; + corrected_position.longitude = longitude; + corrected_position.correction_status = status; + + return corrected_position; +} diff --git a/src/AIS.hpp b/src/AIS.hpp index 48b05b1..f931b70 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -10,6 +10,11 @@ #include #include +#include +#include +#include +#include + 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); From f7a3fde33dbfdd192bc52ba11fea1a90fbe7ad64 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 23 Jan 2025 12:57:58 -0300 Subject: [PATCH 03/20] chore: make speed threshold configurable --- src/AIS.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 2e2bbe9..31cb91a 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -8,6 +8,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) @@ -135,14 +136,12 @@ std::pair vesselToWorldO const std::optional& course_over_ground, double speed_over_ground) { - const double min_speed_threshold = 0.2; - if (yaw.has_value()) { 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() && speed_over_ground >= min_speed_threshold) { + else if (course_over_ground.has_value() && 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}; From f770128d93cdb72682fb244ac1fc309e6945edc9 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 23 Jan 2025 15:54:02 -0300 Subject: [PATCH 04/20] test: add tests and LOG_ERROR messages --- src/AIS.cpp | 26 +++++++++++-- test/test_AIS.cpp | 95 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 118 insertions(+), 3 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 31cb91a..0277250 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -136,12 +137,14 @@ std::pair vesselToWorldO const std::optional& course_over_ground, double speed_over_ground) { - if (yaw.has_value()) { + 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() && speed_over_ground >= MIN_SPEED_THRESHOLD) { + 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}; @@ -195,12 +198,28 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi base::Vector3d const& vessel_reference_position, gps_base::UTMConverter utm_converter) { - ais_base::Position corrected_position; + 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); @@ -209,6 +228,7 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi sensor2vessel_in_world_pos, utm_converter); + ais_base::Position corrected_position; corrected_position.latitude = latitude; corrected_position.longitude = longitude; corrected_position.correction_status = status; diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 8b4cf13..631520c 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -25,6 +25,16 @@ struct AISTest : public ::testing::Test, public iodrivers_base::Fixture uint8_t const* msg_u8 = reinterpret_cast(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 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."); + } +} From cbf14f333a21ac1b025e7a4ae0a46f11c4e24ed5 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Mon, 27 Jan 2025 15:37:27 -0300 Subject: [PATCH 05/20] fix: do not throw error if no correction use std::string instead of constexpr char and fix tests --- src/AIS.cpp | 24 +++++++++++++++--------- test/test_AIS.cpp | 27 ++++++++++++++++----------- 2 files changed, 31 insertions(+), 20 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 0277250..b2fcaaa 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -168,13 +168,21 @@ base::samples::RigidBodyState convertGPSToUTM( const std::optional& 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(); + 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; } @@ -198,12 +206,11 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi 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."; + if (std::isnan(position.yaw.getRad()) && + std::isnan(position.course_over_ground.getRad())) { + std::string 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; } @@ -212,11 +219,10 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi position.speed_over_ground); if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) { - constexpr char error_msg[] = + std::string 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; } diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 631520c..03b99f6 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -318,14 +318,10 @@ TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing) base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); - try { + ais_base::Position corrected_position = 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."); - } + 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) @@ -340,12 +336,21 @@ TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_thresho base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); - try { + ais_base::Position corrected_position = AIS::applyPositionCorrection(position, vessel_reference_position, 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 vessel_reference_position(100.0, 50.0, 0.0); + gps_base::UTMConverter utm_converter = createUTMConverter(); + + try { + AIS::applyPositionCorrection({}, 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."); + ASSERT_STREQ(e.what(), "Position data is unavailable."); } } From defb0b6cdb4817b02dee7ab2f8b184a11baf97a6 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Tue, 28 Jan 2025 15:01:23 -0300 Subject: [PATCH 06/20] chore: use getRad() when checking for nan and log the message directly instead of saving it to a variable --- src/AIS.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index b2fcaaa..f53f9a6 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -137,13 +137,13 @@ std::pair vesselToWorldO const std::optional& course_over_ground, double speed_over_ground) { - if (yaw.has_value() && !std::isnan(yaw.value().getDeg())) { + 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().getDeg()) && + !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())), @@ -208,9 +208,9 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi { if (std::isnan(position.yaw.getRad()) && std::isnan(position.course_over_ground.getRad())) { - std::string error_msg = "Position can't be corrected because both 'yaw' " - "and 'course_over_ground' values are missing."; - LOG_ERROR_S << error_msg << std::endl; + LOG_ERROR_S << "Position can't be corrected because both 'yaw' " + "and 'course_over_ground' values are missing." + << std::endl; return position; } @@ -219,10 +219,9 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi position.speed_over_ground); if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) { - std::string 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; + 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; } From 4c8e3b49fdcb51362fb276d0d3ec587ba39edd23 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Tue, 28 Jan 2025 15:03:34 -0300 Subject: [PATCH 07/20] chore: change variable name to match convention and fix wrong documentation about it --- src/AIS.cpp | 8 ++++---- src/AIS.hpp | 6 +++--- test/test_AIS.cpp | 32 ++++++++++++++++++-------------- 3 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index f53f9a6..5cf711a 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -155,11 +155,11 @@ std::pair vesselToWorldO } } -base::Vector3d sensorToVesselInWorldPose(base::Vector3d sensor2vessel_pos, +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_pos; + sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_in_vessel_pos; return sensor2vessel_in_world_pos; } @@ -203,7 +203,7 @@ std::pair convertUTMToGPSInWorldFrame( } ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position, - base::Vector3d const& vessel_reference_position, + base::Vector3d const& sensor2vessel_in_vessel_pos, gps_base::UTMConverter utm_converter) { if (std::isnan(position.yaw.getRad()) && @@ -226,7 +226,7 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi } auto sensor2vessel_in_world_pos = - sensorToVesselInWorldPose(vessel_reference_position, vessel2world_ori); + sensorToVesselInWorldPose(sensor2vessel_in_vessel_pos, vessel2world_ori); auto [latitude, longitude] = convertUTMToGPSInWorldFrame(convertGPSToUTM(position, utm_converter), diff --git a/src/AIS.hpp b/src/AIS.hpp index f931b70..320ae19 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -54,8 +54,8 @@ namespace nmea0183 { * 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 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 @@ -63,7 +63,7 @@ namespace nmea0183 { */ static ais_base::Position applyPositionCorrection( ais_base::Position const& position, - base::Vector3d const& vessel_reference_position, + base::Vector3d const& sensor2vessel_in_vessel_pos, gps_base::UTMConverter utm_converter); static ais_base::Position getPosition(marnav::ais::message_01 const& message); diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 03b99f6..c87cc80 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -274,11 +274,12 @@ TEST_F(AISTest, it_corrects_position_using_yaw) msg.set_hdg(90); auto position = AIS::getPosition(msg); - base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); + 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, vessel_reference_position, utm_converter); + 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); @@ -295,11 +296,12 @@ TEST_F(AISTest, it_corrects_position_using_cog) msg.set_cog(90); auto position = AIS::getPosition(msg); - base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); + 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, vessel_reference_position, utm_converter); + 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); @@ -315,11 +317,12 @@ TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing) msg.set_sog(0); auto position = AIS::getPosition(msg); - base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); + 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, vessel_reference_position, utm_converter); + 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); } @@ -333,22 +336,23 @@ TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_thresho msg.set_cog(90); auto position = AIS::getPosition(msg); - base::Vector3d vessel_reference_position(100.0, 50.0, 0.0); + 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, vessel_reference_position, utm_converter); + 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 vessel_reference_position(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); try { - AIS::applyPositionCorrection({}, vessel_reference_position, utm_converter); + AIS::applyPositionCorrection({}, sensor2vessel_in_vessel_pos, utm_converter); } catch (const std::runtime_error& e) { ASSERT_STREQ(e.what(), "Position data is unavailable."); From 9d1e750ab096c37dc8dda0780326c555c66be579 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Tue, 28 Jan 2025 15:04:14 -0300 Subject: [PATCH 08/20] fix: ref position relative to the vessel's center and avoid using null values in converter --- src/AIS.cpp | 12 +++++++++--- test/test_AIS.cpp | 4 ++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 5cf711a..30cafbf 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -110,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(message.get_draught()) / 10; info.ship_type = static_cast(message.get_shiptype()); info.epfd_fix = static_cast(message.get_epfd_fix()); info.reference_position = - Eigen::Vector3d(message.get_to_stern(), message.get_to_starboard(), 0); + Eigen::Vector3d(static_cast(get_to_stern - length / 2.0), + static_cast(get_to_starboard - width / 2.0), + 0); info.ensureEnumsValid(); return info; diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index c87cc80..3e0b661 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -28,7 +28,7 @@ struct AISTest : public ::testing::Test, public iodrivers_base::Fixture gps_base::UTMConverter createUTMConverter() { - gps_base::UTMConversionParameters parameters = {Eigen::Vector3d(0, 0, 0), + gps_base::UTMConversionParameters parameters = {Eigen::Vector3d(1, 1, 0), 11, true}; gps_base::UTMConverter converter(parameters); @@ -190,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); } From 8752944fa8488443e20b54019e35d14ab609e7e7 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 29 Jan 2025 14:18:06 -0300 Subject: [PATCH 09/20] chore: give speed thresh a clearer name --- src/AIS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 30cafbf..62ac614 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -9,7 +9,7 @@ using namespace marnav; using namespace nmea0183; double constexpr KNOTS_TO_MS = 0.514444; -double constexpr MIN_SPEED_THRESHOLD = 0.2; +double constexpr MIN_SPEED_FOR_VALID_COURSE = 0.2; AIS::AIS(Driver& driver) : mDriver(driver) @@ -150,7 +150,7 @@ std::pair vesselToWorldO } else if (course_over_ground.has_value() && !std::isnan(course_over_ground.value().getRad()) && - speed_over_ground >= MIN_SPEED_THRESHOLD) { + speed_over_ground >= MIN_SPEED_FOR_VALID_COURSE) { return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.value().getRad(), Eigen::Vector3d::UnitZ())), ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE}; From abb26807ef29775ca29f8f07b64b6dd9b601b08a Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 29 Jan 2025 14:20:28 -0300 Subject: [PATCH 10/20] fix: remove static_cast for ref pos elements floating point types are already implicitly converted into double, if necessary --- src/AIS.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 62ac614..0e2f0c7 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -119,10 +119,9 @@ ais_base::VesselInformation AIS::getVesselInformation(ais::message_05 const& mes info.draft = static_cast(message.get_draught()) / 10; info.ship_type = static_cast(message.get_shiptype()); info.epfd_fix = static_cast(message.get_epfd_fix()); - info.reference_position = - Eigen::Vector3d(static_cast(get_to_stern - length / 2.0), - static_cast(get_to_starboard - width / 2.0), - 0); + info.reference_position = Eigen::Vector3d(get_to_stern - (length / 2.0), + get_to_starboard - (width / 2.0), + 0); info.ensureEnumsValid(); return info; From 22bb4d302364e3f22da1dd9fe77f510934eccb35 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 29 Jan 2025 14:26:15 -0300 Subject: [PATCH 11/20] fix: fix passing arguments, pass utm_converter as const& and write const& instead of const &, for consistency. Pass position as std::optional --- src/AIS.cpp | 73 ++++++++++++++++++++++++----------------------- src/AIS.hpp | 4 +-- test/test_AIS.cpp | 2 +- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 0e2f0c7..9e799b0 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -138,19 +138,18 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes } std::pair vesselToWorldOrientation( - const std::optional& yaw, - const std::optional& course_over_ground, + base::Angle const& yaw, + base::Angle const& 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())), + if (!std::isnan(yaw.getRad())) { + return { + Eigen::Quaterniond(Eigen::AngleAxisd(yaw.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()) && + else if (!std::isnan(course_over_ground.getRad()) && speed_over_ground >= MIN_SPEED_FOR_VALID_COURSE) { - return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.value().getRad(), + return {Eigen::Quaterniond(Eigen::AngleAxisd(course_over_ground.getRad(), Eigen::Vector3d::UnitZ())), ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE}; } @@ -160,8 +159,9 @@ std::pair vesselToWorldO } } -base::Vector3d sensorToVesselInWorldPose(base::Vector3d sensor2vessel_in_vessel_pos, - Eigen::Quaterniond vessel2world_ori) +base::Vector3d sensorToVesselInWorldPose( + base::Vector3d const& sensor2vessel_in_vessel_pos, + Eigen::Quaterniond const& vessel2world_ori) { base::Vector3d sensor2vessel_in_world_pos; sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_in_vessel_pos; @@ -169,20 +169,12 @@ base::Vector3d sensorToVesselInWorldPose(base::Vector3d sensor2vessel_in_vessel_ return sensor2vessel_in_world_pos; } -base::samples::RigidBodyState convertGPSToUTM( - const std::optional& position, - gps_base::UTMConverter utm_converter) +base::samples::RigidBodyState convertGPSToUTM(ais_base::Position const& position, + gps_base::UTMConverter const& 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(); + sensor2world_solution.latitude = position.latitude.getDeg(); + sensor2world_solution.longitude = position.longitude.getDeg(); base::samples::RigidBodyState sensor2world_UTM; sensor2world_UTM.position = @@ -193,9 +185,9 @@ base::samples::RigidBodyState convertGPSToUTM( } std::pair convertUTMToGPSInWorldFrame( - base::samples::RigidBodyState sensor2world_UTM, - base::Vector3d sensor2vessel_in_world_pos, - gps_base::UTMConverter utm_converter) + base::samples::RigidBodyState const& sensor2world_UTM, + base::Vector3d const& sensor2vessel_in_world_pos, + gps_base::UTMConverter const& utm_converter) { base::samples::RigidBodyState vessel2world_UTM; vessel2world_UTM.position = sensor2world_UTM.position + sensor2vessel_in_world_pos; @@ -207,34 +199,43 @@ std::pair convertUTMToGPSInWorldFrame( base::Angle::fromDeg(vessel2world_GPS.longitude)}; } -ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position, +ais_base::Position AIS::applyPositionCorrection( + std::optional position, base::Vector3d const& sensor2vessel_in_vessel_pos, - gps_base::UTMConverter utm_converter) + gps_base::UTMConverter const& 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' " + 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(); + + if (std::isnan(position_value.yaw.getRad()) && + std::isnan(position_value.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 position; + return position_value; } - auto [vessel2world_ori, status] = vesselToWorldOrientation(position.yaw, - position.course_over_ground, - position.speed_over_ground); + auto [vessel2world_ori, status] = vesselToWorldOrientation(position_value.yaw, + position_value.course_over_ground, + position_value.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; + return position_value; } auto sensor2vessel_in_world_pos = sensorToVesselInWorldPose(sensor2vessel_in_vessel_pos, vessel2world_ori); auto [latitude, longitude] = - convertUTMToGPSInWorldFrame(convertGPSToUTM(position, utm_converter), + convertUTMToGPSInWorldFrame(convertGPSToUTM(position_value, utm_converter), sensor2vessel_in_world_pos, utm_converter); diff --git a/src/AIS.hpp b/src/AIS.hpp index 320ae19..17ee7e1 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -62,9 +62,9 @@ namespace nmea0183 { * correction status */ static ais_base::Position applyPositionCorrection( - ais_base::Position const& position, + std::optional position, base::Vector3d const& sensor2vessel_in_vessel_pos, - gps_base::UTMConverter utm_converter); + gps_base::UTMConverter const& utm_converter); static ais_base::Position getPosition(marnav::ais::message_01 const& message); static ais_base::VesselInformation getVesselInformation( diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 3e0b661..8215e9b 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -354,7 +354,7 @@ TEST_F(AISTest, it_throws_runtime_error_if_position_has_no_value) try { AIS::applyPositionCorrection({}, sensor2vessel_in_vessel_pos, utm_converter); } - catch (const std::runtime_error& e) { + catch (std::invalid_argument const& e) { ASSERT_STREQ(e.what(), "Position data is unavailable."); } } From aa9795f8bd947bb8ef11e83ec5f1f3079799de69 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 29 Jan 2025 14:28:05 -0300 Subject: [PATCH 12/20] chore: change log type to debug instead of error because they're expected behaviors, we shouldn't treat them as errors --- src/AIS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 9e799b0..860b547 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -225,7 +225,7 @@ ais_base::Position AIS::applyPositionCorrection( position_value.speed_over_ground); if (status == ais_base::PositionCorrectionStatus::POSITION_RAW) { - LOG_ERROR_S << "Position can't be corrected because 'yaw' value is missing and " + LOG_DEBUG_S << "Position can't be corrected because 'yaw' value is missing and " "'speed_over_ground' is below the threshold." << std::endl; return position_value; From d33ceecc7a741096c6cd2afefa4bcf159171c865 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 30 Jan 2025 14:22:04 -0300 Subject: [PATCH 13/20] chore: remove unnecessary suffix in sensor2vessel_pos --- src/AIS.cpp | 9 ++++----- src/AIS.hpp | 4 ++-- test/test_AIS.cpp | 32 ++++++++++++++------------------ 3 files changed, 20 insertions(+), 25 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 860b547..7e45d83 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -159,12 +159,11 @@ std::pair vesselToWorldO } } -base::Vector3d sensorToVesselInWorldPose( - base::Vector3d const& sensor2vessel_in_vessel_pos, +base::Vector3d sensorToVesselInWorldPose(base::Vector3d const& sensor2vessel_pos, Eigen::Quaterniond const& vessel2world_ori) { base::Vector3d sensor2vessel_in_world_pos; - sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_in_vessel_pos; + sensor2vessel_in_world_pos = vessel2world_ori * sensor2vessel_pos; return sensor2vessel_in_world_pos; } @@ -201,7 +200,7 @@ std::pair convertUTMToGPSInWorldFrame( ais_base::Position AIS::applyPositionCorrection( std::optional position, - base::Vector3d const& sensor2vessel_in_vessel_pos, + base::Vector3d const& sensor2vessel_pos, gps_base::UTMConverter const& utm_converter) { if (!position.has_value()) { @@ -232,7 +231,7 @@ ais_base::Position AIS::applyPositionCorrection( } auto sensor2vessel_in_world_pos = - sensorToVesselInWorldPose(sensor2vessel_in_vessel_pos, vessel2world_ori); + sensorToVesselInWorldPose(sensor2vessel_pos, vessel2world_ori); auto [latitude, longitude] = convertUTMToGPSInWorldFrame(convertGPSToUTM(position_value, utm_converter), diff --git a/src/AIS.hpp b/src/AIS.hpp index 17ee7e1..8be34aa 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -54,7 +54,7 @@ namespace nmea0183 { * offset * * @param position the AIS position to be corrected - * @param sensor2vessel_in_vessel_pos The position of the sensor relative to the + * @param sensor2vessel_pos The position of the sensor relative to the * vessel * @param utm_converter The UTM converter * @@ -63,7 +63,7 @@ namespace nmea0183 { */ static ais_base::Position applyPositionCorrection( std::optional position, - base::Vector3d const& sensor2vessel_in_vessel_pos, + base::Vector3d const& sensor2vessel_pos, gps_base::UTMConverter const& utm_converter); static ais_base::Position getPosition(marnav::ais::message_01 const& message); diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 8215e9b..d7b872e 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -274,12 +274,11 @@ TEST_F(AISTest, it_corrects_position_using_yaw) msg.set_hdg(90); auto position = AIS::getPosition(msg); - base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_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); + ais_base::Position corrected_position = + AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4); ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4); @@ -296,12 +295,11 @@ TEST_F(AISTest, it_corrects_position_using_cog) msg.set_cog(90); auto position = AIS::getPosition(msg); - base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_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); + ais_base::Position corrected_position = + AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4); ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4); @@ -317,12 +315,11 @@ TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing) msg.set_sog(0); auto position = AIS::getPosition(msg); - base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_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); + ais_base::Position corrected_position = + AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); ASSERT_EQ(corrected_position.correction_status, ais_base::PositionCorrectionStatus::POSITION_RAW); } @@ -336,23 +333,22 @@ TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_thresho msg.set_cog(90); auto position = AIS::getPosition(msg); - base::Vector3d sensor2vessel_in_vessel_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_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); + ais_base::Position corrected_position = + AIS::applyPositionCorrection(position, sensor2vessel_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); + base::Vector3d sensor2vessel_pos(100.0, 50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); try { - AIS::applyPositionCorrection({}, sensor2vessel_in_vessel_pos, utm_converter); + AIS::applyPositionCorrection({}, sensor2vessel_pos, utm_converter); } catch (std::invalid_argument const& e) { ASSERT_STREQ(e.what(), "Position data is unavailable."); From bb37d997c85db264c24f9014e8a8cd367cb06dcf Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Thu, 30 Jan 2025 15:42:59 -0300 Subject: [PATCH 14/20] fix/chore: fix passing arguments and refactor code --- src/AIS.cpp | 107 +++++++++++++++++++++++----------------------------- src/AIS.hpp | 26 +++++++++++-- 2 files changed, 70 insertions(+), 63 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 7e45d83..d7e93bc 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -110,17 +110,17 @@ 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); - 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; + 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(message.get_draught()) / 10; info.ship_type = static_cast(message.get_shiptype()); info.epfd_fix = static_cast(message.get_epfd_fix()); - info.reference_position = Eigen::Vector3d(get_to_stern - (length / 2.0), - get_to_starboard - (width / 2.0), + info.reference_position = Eigen::Vector3d(distance_to_stern - (length / 2.0), + distance_to_starboard - (width / 2.0), 0); info.ensureEnumsValid(); @@ -137,10 +137,10 @@ ais_base::VoyageInformation AIS::getVoyageInformation(ais::message_05 const& mes return info; } -std::pair vesselToWorldOrientation( - base::Angle const& yaw, - base::Angle const& course_over_ground, - double speed_over_ground) +std::pair AIS:: + selectVesselHeadingSource(base::Angle const& yaw, + base::Angle const& course_over_ground, + double speed_over_ground) { if (!std::isnan(yaw.getRad())) { return { @@ -159,89 +159,76 @@ std::pair vesselToWorldO } } -base::Vector3d sensorToVesselInWorldPose(base::Vector3d const& sensor2vessel_pos, - Eigen::Quaterniond const& 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(ais_base::Position const& position, +base::Vector3d convertPositionInGPSToUTM(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(); - base::samples::RigidBodyState sensor2world_UTM; - sensor2world_UTM.position = - utm_converter.convertToUTM(sensor2world_solution).position; - sensor2world_UTM.time = base::Time::now(); + auto sensor2world = utm_converter.convertToUTM(sensor2world_solution); + + return sensor2world.position; +} - return sensor2world_UTM; +base::Vector3d getVesselPositionInWorldFrame(base::Vector3d const& sensor2vessel_pos, + Eigen::Quaterniond const& vessel2world_ori) +{ + return -(vessel2world_ori * sensor2vessel_pos); } std::pair convertUTMToGPSInWorldFrame( - base::samples::RigidBodyState const& sensor2world_UTM, - base::Vector3d const& sensor2vessel_in_world_pos, + base::Vector3d const& sensor2world_pos, + base::Vector3d const& vessel2sensor_in_world_pos, gps_base::UTMConverter const& utm_converter) { - base::samples::RigidBodyState vessel2world_UTM; - vessel2world_UTM.position = sensor2world_UTM.position + sensor2vessel_in_world_pos; + base::samples::RigidBodyState vessel2world; + vessel2world.position = sensor2world_pos + vessel2sensor_in_world_pos; - gps_base::Solution vessel2world_GPS; - vessel2world_GPS = utm_converter.convertUTMToGPS(vessel2world_UTM); + gps_base::Solution vessel2world_gps; + vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); - return {base::Angle::fromDeg(vessel2world_GPS.latitude), - base::Angle::fromDeg(vessel2world_GPS.longitude)}; + return {base::Angle::fromDeg(vessel2world_gps.latitude), + base::Angle::fromDeg(vessel2world_gps.longitude)}; } -ais_base::Position AIS::applyPositionCorrection( - std::optional position, +ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position, base::Vector3d const& sensor2vessel_pos, gps_base::UTMConverter const& 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(); + auto sensor2world_gps = position; - if (std::isnan(position_value.yaw.getRad()) && - std::isnan(position_value.course_over_ground.getRad())) { + if (std::isnan(sensor2world_gps.yaw.getRad()) && + std::isnan(sensor2world_gps.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 position_value; + return sensor2world_gps; } - auto [vessel2world_ori, status] = vesselToWorldOrientation(position_value.yaw, - position_value.course_over_ground, - position_value.speed_over_ground); + auto [vessel2world_ori, status] = selectVesselHeadingSource(sensor2world_gps.yaw, + sensor2world_gps.course_over_ground, + sensor2world_gps.speed_over_ground); 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; - return position_value; + return sensor2world_gps; } - auto sensor2vessel_in_world_pos = - sensorToVesselInWorldPose(sensor2vessel_pos, vessel2world_ori); + auto sensor2world_pos = convertPositionInGPSToUTM(sensor2world_gps, utm_converter); + + auto vessel2sensor_in_world_pos = + getVesselPositionInWorldFrame(sensor2vessel_pos, vessel2world_ori); - auto [latitude, longitude] = - convertUTMToGPSInWorldFrame(convertGPSToUTM(position_value, utm_converter), - sensor2vessel_in_world_pos, - utm_converter); + auto [latitude, longitude] = convertUTMToGPSInWorldFrame(sensor2world_pos, + vessel2sensor_in_world_pos, + utm_converter); - ais_base::Position corrected_position; - corrected_position.latitude = latitude; - corrected_position.longitude = longitude; - corrected_position.correction_status = status; + sensor2world_gps.latitude = latitude; + sensor2world_gps.longitude = longitude; + sensor2world_gps.correction_status = status; - return corrected_position; + return sensor2world_gps; } diff --git a/src/AIS.hpp b/src/AIS.hpp index 8be34aa..841bfb3 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -53,19 +53,39 @@ namespace nmea0183 { * Applies position correction using the vessel reference position and the sensor * offset * - * @param position the AIS position to be corrected + * @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 AIS position with updated latitude, longitude, and + * @return The corrected vessel position with updated latitude, longitude, and * correction status */ static ais_base::Position applyPositionCorrection( - std::optional position, + 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 + 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); From 926f3f763b7978562ee216e4a33b34328976a1bf Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Tue, 4 Feb 2025 13:38:28 -0300 Subject: [PATCH 15/20] fix: write more robust tests and add tests --- test/test_AIS.cpp | 148 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 110 insertions(+), 38 deletions(-) diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index d7b872e..1736aa6 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -265,57 +265,81 @@ TEST_F(AISTest, it_converts_marnav_message05_into_a_VoyageInformation) ASSERT_EQ("DEST", info.destination); } -TEST_F(AISTest, it_corrects_position_using_yaw) +TEST_F(AISTest, it_selects_yaw_as_vessel_heading_source_if_available) { 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_pos(100.0, 50.0, 0.0); - gps_base::UTMConverter utm_converter = createUTMConverter(); - - ais_base::Position corrected_position = - AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); + auto [ori, status] = AIS::selectVesselHeadingSource(position.yaw, + position.course_over_ground, + position.speed_over_ground); - 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, + ASSERT_NEAR(ori.z(), -0.7071068, 1e-3); + ASSERT_NEAR(ori.w(), 0.7071068, 1e-3); + ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING); } -TEST_F(AISTest, it_corrects_position_using_cog) +TEST_F(AISTest, + it_selects_cog_as_vessel_heading_source_if_no_yaw_and_sog_is_above_threshold_for_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_pos(100.0, 50.0, 0.0); - gps_base::UTMConverter utm_converter = createUTMConverter(); + auto [ori, status] = AIS::selectVesselHeadingSource(position.yaw, + position.course_over_ground, + position.speed_over_ground); - ais_base::Position corrected_position = - AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); + ASSERT_NEAR(ori.z(), -0.7071068, 1e-3); + ASSERT_NEAR(ori.w(), 0.7071068, 1e-3); + ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE); +} - 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_selects_identity_as_vessel_heading_source_if_no_yaw_and_sog_is_below_threshold_for_cog) +{ + ais::message_01 msg; + msg.set_sog(0.1); + msg.set_cog(90); + auto position = AIS::getPosition(msg); + + auto [ori, status] = AIS::selectVesselHeadingSource(position.yaw, + position.course_over_ground, + position.speed_over_ground); + + ASSERT_EQ(ori.z(), 0); + ASSERT_EQ(ori.w(), 1); + ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_RAW); } -TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing) +TEST_F(AISTest, it_selects_identity_as_vessel_heading_source_if_no_yaw_or_cog) +{ + ais::message_01 msg; + auto position = AIS::getPosition(msg); + + auto [ori, status] = AIS::selectVesselHeadingSource(position.yaw, + position.course_over_ground, + position.speed_over_ground); + + ASSERT_EQ(ori.z(), 0); + ASSERT_EQ(ori.w(), 1); + ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_RAW); +} + +TEST_F(AISTest, it_does_no_correction_if_no_yaw_and_sog_is_below_threshold_for_cog) { ais::message_01 msg; msg.set_latitude(geo::latitude(45)); msg.set_longitude(geo::longitude(-120)); - msg.set_sog(0); + msg.set_sog(0.1); + msg.set_cog(90); auto position = AIS::getPosition(msg); - base::Vector3d sensor2vessel_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_pos(-100.0, -50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); ais_base::Position corrected_position = @@ -324,16 +348,14 @@ TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing) ais_base::PositionCorrectionStatus::POSITION_RAW); } -TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_threshold) +TEST_F(AISTest, it_does_no_correction_if_no_yaw_or_cog) { 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_pos(100.0, 50.0, 0.0); + base::Vector3d sensor2vessel_pos(-100.0, -50.0, 0.0); gps_base::UTMConverter utm_converter = createUTMConverter(); ais_base::Position corrected_position = @@ -342,15 +364,65 @@ TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_thresho ais_base::PositionCorrectionStatus::POSITION_RAW); } -TEST_F(AISTest, it_throws_runtime_error_if_position_has_no_value) +TEST_F(AISTest, it_corrects_position_using_yaw) { - base::Vector3d sensor2vessel_pos(100.0, 50.0, 0.0); - gps_base::UTMConverter utm_converter = createUTMConverter(); + auto utm_converter = createUTMConverter(); + base::samples::RigidBodyState vessel2world, sensor2world; + vessel2world.position = base::Vector3d(10, 10, 0); + sensor2world.position = base::Vector3d(9, 11, 0); + auto sensor2world_gps = utm_converter.convertUTMToGPS(sensor2world); + + ais::message_01 msg_position; + msg_position.set_latitude(geo::latitude(sensor2world_gps.latitude)); + msg_position.set_longitude(geo::longitude(sensor2world_gps.longitude)); + msg_position.set_hdg(90); + auto position = AIS::getPosition(msg_position); + + ais::message_05 msg_info; + msg_info.set_to_port(9); + msg_info.set_to_starboard(11); + msg_info.set_to_bow(21); + msg_info.set_to_stern(19); + auto info = AIS::getVesselInformation(msg_info); + + auto corrected_position = + AIS::applyPositionCorrection(position, info.reference_position, utm_converter); + + auto vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); + ASSERT_NEAR(vessel2world_gps.latitude, corrected_position.latitude.getDeg(), 1e-3); + ASSERT_NEAR(vessel2world_gps.longitude, corrected_position.longitude.getDeg(), 1e-3); + ASSERT_EQ(corrected_position.correction_status, + ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING); +} - try { - AIS::applyPositionCorrection({}, sensor2vessel_pos, utm_converter); - } - catch (std::invalid_argument const& e) { - ASSERT_STREQ(e.what(), "Position data is unavailable."); - } +TEST_F(AISTest, it_corrects_position_using_cog) +{ + auto utm_converter = createUTMConverter(); + base::samples::RigidBodyState vessel2world, sensor2world; + vessel2world.position = base::Vector3d(10, 10, 0); + sensor2world.position = base::Vector3d(9, 11, 0); + auto sensor2world_gps = utm_converter.convertUTMToGPS(sensor2world); + + ais::message_01 msg_position; + msg_position.set_latitude(geo::latitude(sensor2world_gps.latitude)); + msg_position.set_longitude(geo::longitude(sensor2world_gps.longitude)); + msg_position.set_sog(0.5); + msg_position.set_cog(90); + auto position = AIS::getPosition(msg_position); + + ais::message_05 msg_info; + msg_info.set_to_port(9); + msg_info.set_to_starboard(11); + msg_info.set_to_bow(21); + msg_info.set_to_stern(19); + auto info = AIS::getVesselInformation(msg_info); + + auto corrected_position = + AIS::applyPositionCorrection(position, info.reference_position, utm_converter); + + auto vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); + ASSERT_NEAR(vessel2world_gps.latitude, corrected_position.latitude.getDeg(), 1e-3); + ASSERT_NEAR(vessel2world_gps.longitude, corrected_position.longitude.getDeg(), 1e-3); + ASSERT_EQ(corrected_position.correction_status, + ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE); } From 6bfcd04c7f185d43b646f55b070e0321a98a1f09 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 5 Feb 2025 11:07:39 -0300 Subject: [PATCH 16/20] chore: use snake_case for variables --- src/AIS.cpp | 10 +++++----- src/AIS.hpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index d7e93bc..8678249 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -12,14 +12,14 @@ 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::readMessage() { while (true) { - auto sentence = mDriver.readSentence(); + auto sentence = m_driver.readSentence(); auto msg = processSentence(*sentence); if (msg) { return msg; @@ -29,7 +29,7 @@ unique_ptr AIS::readMessage() uint32_t AIS::getDiscardedSentenceCount() const { - return mDiscardedSentenceCount; + return m_discarded_sentence_count; } unique_ptr AIS::processSentence(nmea::sentence const& sentence) @@ -43,12 +43,12 @@ unique_ptr 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(); } } diff --git a/src/AIS.hpp b/src/AIS.hpp index 841bfb3..1d764cc 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -17,8 +17,8 @@ namespace nmea0183 { class AIS { - uint32_t mDiscardedSentenceCount = 0; - Driver& mDriver; + uint32_t m_discarded_sentence_count = 0; + Driver& m_driver; std::vector> payloads; public: From dd6fb6d47e747983d990215c4a7361e448a15985 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 5 Feb 2025 11:16:28 -0300 Subject: [PATCH 17/20] chore: propagate status --- src/AIS.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/AIS.cpp b/src/AIS.cpp index 8678249..06b5c65 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -214,6 +214,7 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& positi LOG_DEBUG_S << "Position can't be corrected because 'yaw' value is missing and " "'speed_over_ground' is below the threshold." << std::endl; + sensor2world_gps.correction_status = status; return sensor2world_gps; } From c144c10049203a0f12b755ce706891103ec90972 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 5 Feb 2025 15:12:15 -0300 Subject: [PATCH 18/20] fix/chore: fix nomenclature and move transform code Having "_gps" as a suffix for the sensor and vessel positions in GPS coordinates was intended to distinguish GPS coordinates from UTM ones. That was a confusing nomenclature, but as I had no better idea, I just went on with it. After discussing the matter, I agreed naming these variables simply "sensor_pos" and "vessel_pos" were a satisfactory way to make this distinction, as the type "ais_base::Position" is known to have coordinates in the GPS system. So this solved the nomenclature problem. Part of the transformation to get the vessel position in the world frame was left inside the "convertUTMToGPS" function. This was fixed by moving it to the now called "computeVesselPositionInWorldFrame". --- src/AIS.cpp | 61 +++++++++++++++++++++++++++-------------------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index 06b5c65..c23a606 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -159,7 +159,7 @@ std::pair AIS:: } } -base::Vector3d convertPositionInGPSToUTM(ais_base::Position const& position, +base::Vector3d convertGPSToUTM(ais_base::Position const& position, gps_base::UTMConverter const& utm_converter) { gps_base::Solution sensor2world_solution; @@ -171,20 +171,23 @@ base::Vector3d convertPositionInGPSToUTM(ais_base::Position const& position, return sensor2world.position; } -base::Vector3d getVesselPositionInWorldFrame(base::Vector3d const& sensor2vessel_pos, - Eigen::Quaterniond const& vessel2world_ori) +base::Vector3d computeVesselPositionInWorldFrame( + base::Vector3d const& sensor2vessel_pos, + Eigen::Quaterniond const& vessel2world_ori, + base::Vector3d const& sensor2world_pos) { - return -(vessel2world_ori * sensor2vessel_pos); + const auto sensor2world_ori = vessel2world_ori; + base::samples::RigidBodyState vessel2world; + vessel2world.position = sensor2world_pos - (sensor2world_ori * sensor2vessel_pos); + return vessel2world.position; } -std::pair convertUTMToGPSInWorldFrame( - base::Vector3d const& sensor2world_pos, - base::Vector3d const& vessel2sensor_in_world_pos, +std::pair convertUTMToGPS( + base::Vector3d const& vessel2world_pos, gps_base::UTMConverter const& utm_converter) { base::samples::RigidBodyState vessel2world; - vessel2world.position = sensor2world_pos + vessel2sensor_in_world_pos; - + vessel2world.position = vessel2world_pos; gps_base::Solution vessel2world_gps; vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); @@ -192,44 +195,42 @@ std::pair convertUTMToGPSInWorldFrame( base::Angle::fromDeg(vessel2world_gps.longitude)}; } -ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& position, +ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& sensor_pos, base::Vector3d const& sensor2vessel_pos, gps_base::UTMConverter const& utm_converter) { - auto sensor2world_gps = position; - - if (std::isnan(sensor2world_gps.yaw.getRad()) && - std::isnan(sensor2world_gps.course_over_ground.getRad())) { + 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 sensor2world_gps; + return sensor_pos; } - auto [vessel2world_ori, status] = selectVesselHeadingSource(sensor2world_gps.yaw, - sensor2world_gps.course_over_ground, - sensor2world_gps.speed_over_ground); + 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; - sensor2world_gps.correction_status = status; - return sensor2world_gps; + vessel_pos.correction_status = status; + return vessel_pos; } - auto sensor2world_pos = convertPositionInGPSToUTM(sensor2world_gps, utm_converter); + auto sensor2world_pos = convertGPSToUTM(sensor_pos, utm_converter); - auto vessel2sensor_in_world_pos = - getVesselPositionInWorldFrame(sensor2vessel_pos, vessel2world_ori); + auto vessel2world_pos = computeVesselPositionInWorldFrame(sensor2vessel_pos, + vessel2world_ori, + sensor2world_pos); - auto [latitude, longitude] = convertUTMToGPSInWorldFrame(sensor2world_pos, - vessel2sensor_in_world_pos, - utm_converter); + auto [latitude, longitude] = convertUTMToGPS(vessel2world_pos, utm_converter); - sensor2world_gps.latitude = latitude; - sensor2world_gps.longitude = longitude; - sensor2world_gps.correction_status = status; + vessel_pos.latitude = latitude; + vessel_pos.longitude = longitude; + vessel_pos.correction_status = status; - return sensor2world_gps; + return vessel_pos; } From 899444bccbba4843f138552f6370e6fd443bcf17 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Wed, 5 Feb 2025 15:14:01 -0300 Subject: [PATCH 19/20] test: add checks for timestamp in the tests and rename vessel2world_gps to vessel_pos, as is done in the code --- test/test_AIS.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 1736aa6..8a2ab7a 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -346,6 +346,7 @@ TEST_F(AISTest, it_does_no_correction_if_no_yaw_and_sog_is_below_threshold_for_c AIS::applyPositionCorrection(position, sensor2vessel_pos, utm_converter); ASSERT_EQ(corrected_position.correction_status, ais_base::PositionCorrectionStatus::POSITION_RAW); + ASSERT_EQ(position.time, corrected_position.time); } TEST_F(AISTest, it_does_no_correction_if_no_yaw_or_cog) @@ -388,11 +389,12 @@ TEST_F(AISTest, it_corrects_position_using_yaw) auto corrected_position = AIS::applyPositionCorrection(position, info.reference_position, utm_converter); - auto vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); - ASSERT_NEAR(vessel2world_gps.latitude, corrected_position.latitude.getDeg(), 1e-3); - ASSERT_NEAR(vessel2world_gps.longitude, corrected_position.longitude.getDeg(), 1e-3); + auto vessel_pos = utm_converter.convertUTMToGPS(vessel2world); + ASSERT_NEAR(vessel_pos.latitude, corrected_position.latitude.getDeg(), 1e-3); + ASSERT_NEAR(vessel_pos.longitude, corrected_position.longitude.getDeg(), 1e-3); ASSERT_EQ(corrected_position.correction_status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING); + ASSERT_EQ(position.time, corrected_position.time); } TEST_F(AISTest, it_corrects_position_using_cog) @@ -420,9 +422,10 @@ TEST_F(AISTest, it_corrects_position_using_cog) auto corrected_position = AIS::applyPositionCorrection(position, info.reference_position, utm_converter); - auto vessel2world_gps = utm_converter.convertUTMToGPS(vessel2world); - ASSERT_NEAR(vessel2world_gps.latitude, corrected_position.latitude.getDeg(), 1e-3); - ASSERT_NEAR(vessel2world_gps.longitude, corrected_position.longitude.getDeg(), 1e-3); + auto vessel_pos = utm_converter.convertUTMToGPS(vessel2world); + ASSERT_NEAR(vessel_pos.latitude, corrected_position.latitude.getDeg(), 1e-3); + ASSERT_NEAR(vessel_pos.longitude, corrected_position.longitude.getDeg(), 1e-3); ASSERT_EQ(corrected_position.correction_status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE); + ASSERT_EQ(position.time, corrected_position.time); } From 35415637f3d1d7d89d7f58dc0fdb460f0b0d41c5 Mon Sep 17 00:00:00 2001 From: eduardacoppo Date: Fri, 7 Feb 2025 17:05:17 -0300 Subject: [PATCH 20/20] chore: minor fixes --- src/AIS.cpp | 12 ++++++------ src/AIS.hpp | 3 --- test/test_AIS.cpp | 16 ++++++++-------- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/AIS.cpp b/src/AIS.cpp index c23a606..b762207 100644 --- a/src/AIS.cpp +++ b/src/AIS.cpp @@ -171,15 +171,14 @@ base::Vector3d convertGPSToUTM(ais_base::Position const& position, return sensor2world.position; } -base::Vector3d computeVesselPositionInWorldFrame( - base::Vector3d const& sensor2vessel_pos, +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; + base::Vector3d vessel2world_pos; + vessel2world_pos = sensor2world_pos - (sensor2world_ori * sensor2vessel_pos); + return vessel2world_pos; } std::pair convertUTMToGPS( @@ -199,11 +198,13 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& sensor base::Vector3d const& sensor2vessel_pos, gps_base::UTMConverter const& utm_converter) { + auto vessel_pos = sensor_pos; 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; + vessel_pos.correction_status = ais_base::PositionCorrectionStatus::POSITION_RAW; return sensor_pos; } @@ -211,7 +212,6 @@ ais_base::Position AIS::applyPositionCorrection(ais_base::Position const& sensor 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." diff --git a/src/AIS.hpp b/src/AIS.hpp index 1d764cc..62a2ca6 100644 --- a/src/AIS.hpp +++ b/src/AIS.hpp @@ -10,10 +10,7 @@ #include #include -#include -#include #include -#include namespace nmea0183 { class AIS { diff --git a/test/test_AIS.cpp b/test/test_AIS.cpp index 8a2ab7a..a3f4991 100644 --- a/test/test_AIS.cpp +++ b/test/test_AIS.cpp @@ -276,8 +276,8 @@ TEST_F(AISTest, it_selects_yaw_as_vessel_heading_source_if_available) position.course_over_ground, position.speed_over_ground); - ASSERT_NEAR(ori.z(), -0.7071068, 1e-3); - ASSERT_NEAR(ori.w(), 0.7071068, 1e-3); + ASSERT_TRUE(ori.isApprox( + Eigen::Quaterniond(Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitZ())))); ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING); } @@ -294,8 +294,8 @@ TEST_F(AISTest, position.course_over_ground, position.speed_over_ground); - ASSERT_NEAR(ori.z(), -0.7071068, 1e-3); - ASSERT_NEAR(ori.w(), 0.7071068, 1e-3); + ASSERT_TRUE(ori.isApprox( + Eigen::Quaterniond(Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitZ())))); ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE); } @@ -311,8 +311,8 @@ TEST_F(AISTest, position.course_over_ground, position.speed_over_ground); - ASSERT_EQ(ori.z(), 0); - ASSERT_EQ(ori.w(), 1); + ASSERT_TRUE( + ori.isApprox(Eigen::Quaterniond(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())))); ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_RAW); } @@ -325,8 +325,8 @@ TEST_F(AISTest, it_selects_identity_as_vessel_heading_source_if_no_yaw_or_cog) position.course_over_ground, position.speed_over_ground); - ASSERT_EQ(ori.z(), 0); - ASSERT_EQ(ori.w(), 1); + ASSERT_TRUE( + ori.isApprox(Eigen::Quaterniond(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())))); ASSERT_EQ(status, ais_base::PositionCorrectionStatus::POSITION_RAW); }