Skip to content

Commit

Permalink
chore: change variable name to match convention
Browse files Browse the repository at this point in the history
and fix wrong documentation about it
  • Loading branch information
eduardacoppo committed Jan 28, 2025
1 parent defb0b6 commit 4c8e3b4
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 21 deletions.
8 changes: 4 additions & 4 deletions src/AIS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -155,11 +155,11 @@ std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> 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;
}
Expand Down Expand Up @@ -203,7 +203,7 @@ std::pair<base::Angle, base::Angle> 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()) &&
Expand All @@ -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),
Expand Down
6 changes: 3 additions & 3 deletions src/AIS.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,16 +54,16 @@ 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
* correction status
*/
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);
Expand Down
32 changes: 18 additions & 14 deletions test/test_AIS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
}
Expand All @@ -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.");
Expand Down

0 comments on commit 4c8e3b4

Please sign in to comment.