Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore: move position correction code to lib #8

Merged
merged 20 commits into from
Feb 13, 2025
Merged
Changes from 1 commit
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
87d41c1
chore: set cpp17 version in CMakeLists
eduardacoppo Jan 23, 2025
ae7baa6
chore: move position correction code to lib
eduardacoppo Jan 23, 2025
f7a3fde
chore: make speed threshold configurable
eduardacoppo Jan 23, 2025
f770128
test: add tests and LOG_ERROR messages
eduardacoppo Jan 23, 2025
cbf14f3
fix: do not throw error if no correction
eduardacoppo Jan 27, 2025
defb0b6
chore: use getRad() when checking for nan
eduardacoppo Jan 28, 2025
4c8e3b4
chore: change variable name to match convention
eduardacoppo Jan 28, 2025
9d1e750
fix: ref position relative to the vessel's center
eduardacoppo Jan 28, 2025
8752944
chore: give speed thresh a clearer name
eduardacoppo Jan 29, 2025
abb2680
fix: remove static_cast for ref pos elements
eduardacoppo Jan 29, 2025
22bb4d3
fix: fix passing arguments,
eduardacoppo Jan 29, 2025
aa9795f
chore: change log type to debug instead of error
eduardacoppo Jan 29, 2025
d33ceec
chore: remove unnecessary suffix in sensor2vessel_pos
eduardacoppo Jan 30, 2025
bb37d99
fix/chore: fix passing arguments and refactor code
eduardacoppo Jan 30, 2025
926f3f7
fix: write more robust tests and add tests
eduardacoppo Feb 4, 2025
6bfcd04
chore: use snake_case for variables
eduardacoppo Feb 5, 2025
dd6fb6d
chore: propagate status
eduardacoppo Feb 5, 2025
c144c10
fix/chore: fix nomenclature and move transform code
eduardacoppo Feb 5, 2025
899444b
test: add checks for timestamp in the tests
eduardacoppo Feb 5, 2025
3541563
chore: minor fixes
eduardacoppo Feb 7, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
test: add tests and LOG_ERROR messages
eduardacoppo committed Jan 23, 2025
commit f770128d93cdb72682fb244ac1fc309e6945edc9
26 changes: 23 additions & 3 deletions src/AIS.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <base-logging/Logging.hpp>
#include <marnav/ais/ais.hpp>
#include <marnav/nmea/vdm.hpp>
#include <nmea0183/AIS.hpp>
@@ -136,12 +137,14 @@ std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> vesselToWorldO
const std::optional<base::Angle>& 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;
95 changes: 95 additions & 0 deletions test/test_AIS.cpp
Original file line number Diff line number Diff line change
@@ -25,6 +25,16 @@ struct AISTest : public ::testing::Test, public iodrivers_base::Fixture<Driver>
uint8_t const* msg_u8 = reinterpret_cast<uint8_t const*>(msg.c_str());
pushDataToDriver(msg_u8, msg_u8 + msg.size());
}

gps_base::UTMConverter createUTMConverter()
{
gps_base::UTMConversionParameters parameters = {Eigen::Vector3d(0, 0, 0),
11,
true};
gps_base::UTMConverter converter(parameters);

return converter;
}
};

const std::vector<std::string> ais_strings = {
@@ -254,3 +264,88 @@ TEST_F(AISTest, it_converts_marnav_message05_into_a_VoyageInformation)
ASSERT_EQ(7890, info.imo);
ASSERT_EQ("DEST", info.destination);
}

TEST_F(AISTest, it_corrects_position_using_yaw)
{
ais::message_01 msg;
msg.set_latitude(geo::latitude(45));
msg.set_longitude(geo::longitude(-120));
msg.set_sog(0);
msg.set_hdg(90);
auto position = AIS::getPosition(msg);

base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
gps_base::UTMConverter utm_converter = createUTMConverter();

ais_base::Position corrected_position =
AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);

ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4);
ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4);
ASSERT_EQ(corrected_position.correction_status,
ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_HEADING);
}

TEST_F(AISTest, it_corrects_position_using_cog)
{
ais::message_01 msg;
msg.set_latitude(geo::latitude(45));
msg.set_longitude(geo::longitude(-120));
msg.set_sog(0.5);
msg.set_cog(90);
auto position = AIS::getPosition(msg);

base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
gps_base::UTMConverter utm_converter = createUTMConverter();

ais_base::Position corrected_position =
AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);

ASSERT_NEAR(corrected_position.latitude.getDeg(), 44.9991, 1e-4);
ASSERT_NEAR(corrected_position.longitude.getDeg(), -119.9993, 1e-4);
ASSERT_EQ(corrected_position.correction_status,
ais_base::PositionCorrectionStatus::POSITION_CENTERED_USING_COURSE);
}

TEST_F(AISTest, it_does_no_correction_if_both_yaw_and_cog_are_missing)
{
ais::message_01 msg;
msg.set_latitude(geo::latitude(45));
msg.set_longitude(geo::longitude(-120));
msg.set_sog(0);
auto position = AIS::getPosition(msg);

base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
gps_base::UTMConverter utm_converter = createUTMConverter();

try {
AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
}
catch (const std::runtime_error& e) {
ASSERT_STREQ(e.what(),
"Position can't be corrected because both 'yaw' "
"and 'course_over_ground' values are missing.");
}
}

TEST_F(AISTest, it_does_no_correction_if_yaw_is_missing_and_sog_is_below_threshold)
{
ais::message_01 msg;
msg.set_latitude(geo::latitude(45));
msg.set_longitude(geo::longitude(-120));
msg.set_sog(0.1);
msg.set_cog(90);
auto position = AIS::getPosition(msg);

base::Vector3d vessel_reference_position(100.0, 50.0, 0.0);
gps_base::UTMConverter utm_converter = createUTMConverter();

try {
AIS::applyPositionCorrection(position, vessel_reference_position, utm_converter);
}
catch (const std::runtime_error& e) {
ASSERT_STREQ(e.what(),
"Position can't be corrected because 'yaw' value is missing and "
"'speed_over_ground' is below the threshold.");
}
}