From 013c1ed0a5610f8bfaccaeabde1a451aea7f124c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 23 Jan 2019 11:34:04 +0100 Subject: [PATCH 1/2] mission: make mission item comparison less strict I think we shouldn't compare doubles and floats of mission items with == but only compare it to an accuracy that makes sense for the use case. In case of latitude and longitude we are limited to 1e-7 anyway because the MAVLink transport happens over int at 1e7. Included in this change is to make use of the comparison in the ingration test. --- integration_tests/mission.cpp | 22 +------- plugins/mission/mission_item.cpp | 89 ++++++++++++++++++++++++++------ 2 files changed, 73 insertions(+), 38 deletions(-) diff --git a/integration_tests/mission.cpp b/integration_tests/mission.cpp index 9fc05a6bc7..ff331abbe7 100644 --- a/integration_tests/mission.cpp +++ b/integration_tests/mission.cpp @@ -262,27 +262,7 @@ std::shared_ptr add_mission_item(double latitude_deg, void compare_mission_items(const std::shared_ptr original, const std::shared_ptr downloaded) { - EXPECT_NEAR(original->get_latitude_deg(), downloaded->get_latitude_deg(), 1e-6); - EXPECT_NEAR(original->get_longitude_deg(), downloaded->get_longitude_deg(), 1e-6); - - EXPECT_FLOAT_EQ(original->get_relative_altitude_m(), downloaded->get_relative_altitude_m()); - - EXPECT_EQ(original->get_fly_through(), downloaded->get_fly_through()); - if (std::isfinite(original->get_speed_m_s())) { - EXPECT_FLOAT_EQ(original->get_speed_m_s(), downloaded->get_speed_m_s()); - } - - EXPECT_EQ(original->get_camera_action(), downloaded->get_camera_action()); - - if (original->get_camera_action() == MissionItem::CameraAction::START_PHOTO_INTERVAL && - std::isfinite(original->get_camera_photo_interval_s())) { - EXPECT_DOUBLE_EQ(original->get_camera_photo_interval_s(), - downloaded->get_camera_photo_interval_s()); - } - - if (std::isfinite(original->get_loiter_time_s())) { - EXPECT_FLOAT_EQ(original->get_loiter_time_s(), downloaded->get_loiter_time_s()); - } + EXPECT_TRUE(*original == *downloaded); } void pause_and_resume(std::shared_ptr mission) diff --git a/plugins/mission/mission_item.cpp b/plugins/mission/mission_item.cpp index e3b0d16370..b96926c56e 100644 --- a/plugins/mission/mission_item.cpp +++ b/plugins/mission/mission_item.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include "mission_item_impl.h" @@ -127,23 +129,76 @@ std::string MissionItem::to_str(MissionItem::CameraAction camera_action) bool operator==(const MissionItem &lhs, const MissionItem &rhs) { - return (lhs.get_latitude_deg() == rhs.get_latitude_deg() || - (isnan(lhs.get_latitude_deg()) && isnan(rhs.get_latitude_deg()))) && - (lhs.get_longitude_deg() == rhs.get_longitude_deg() || - (isnan(lhs.get_longitude_deg()) && isnan(rhs.get_longitude_deg()))) && - (lhs.get_relative_altitude_m() == rhs.get_relative_altitude_m() || - (isnan(lhs.get_relative_altitude_m()) && isnan(rhs.get_relative_altitude_m()))) && - lhs.get_fly_through() == rhs.get_fly_through() && - (lhs.get_speed_m_s() == rhs.get_speed_m_s() || - (isnan(lhs.get_speed_m_s()) && isnan(rhs.get_speed_m_s()))) && - (lhs.get_gimbal_pitch_deg() == rhs.get_gimbal_pitch_deg() || - (isnan(lhs.get_gimbal_pitch_deg()) && isnan(rhs.get_gimbal_pitch_deg()))) && - (lhs.get_gimbal_yaw_deg() == rhs.get_gimbal_yaw_deg() || - (isnan(lhs.get_gimbal_yaw_deg()) && isnan(rhs.get_gimbal_yaw_deg()))) && - (lhs.get_loiter_time_s() == rhs.get_loiter_time_s() || - (isnan(lhs.get_loiter_time_s()) && isnan(rhs.get_loiter_time_s()))) && - lhs.get_camera_action() == rhs.get_camera_action() && - lhs.get_camera_photo_interval_s() == rhs.get_camera_photo_interval_s(); + // For latitude and longitude we expect precision down to 1e-7 because the + // underlying transport happens with int at 1e7. + static constexpr double lat_lon_epsilon = 1e7; + + if (!(isnan(lhs.get_latitude_deg()) && isnan(rhs.get_latitude_deg())) && + std::abs(lhs.get_latitude_deg() - rhs.get_latitude_deg()) > lat_lon_epsilon) { + // LogDebug() << "Latitude different"; + return false; + } + + if (!(isnan(lhs.get_longitude_deg()) && isnan(rhs.get_longitude_deg())) && + std::abs(lhs.get_latitude_deg() - rhs.get_latitude_deg()) > lat_lon_epsilon) { + // LogDebug() << "Longitude different"; + return false; + } + + if (!(isnan(lhs.get_relative_altitude_m()) && isnan(rhs.get_relative_altitude_m())) && + std::fabs(lhs.get_relative_altitude_m() - rhs.get_relative_altitude_m()) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Relative altitude different"; + return false; + } + + if (lhs.get_fly_through() != rhs.get_fly_through()) { + // LogDebug() << "Fly-through different." + return false; + } + + if (!(isnan(lhs.get_speed_m_s()) && isnan(rhs.get_speed_m_s())) && + std::fabs(lhs.get_speed_m_s() - rhs.get_speed_m_s()) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Speed different"; + return false; + } + + if (!(isnan(lhs.get_gimbal_pitch_deg()) && isnan(rhs.get_gimbal_pitch_deg())) && + std::fabs(lhs.get_gimbal_pitch_deg() - rhs.get_gimbal_pitch_deg()) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Gimbal pitch different"; + return false; + } + + if (!(isnan(lhs.get_gimbal_yaw_deg()) && isnan(rhs.get_gimbal_yaw_deg())) && + std::fabs(lhs.get_gimbal_yaw_deg() - rhs.get_gimbal_yaw_deg()) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Gimbal yaw different"; + return false; + } + + if (!(isnan(lhs.get_loiter_time_s()) && isnan(rhs.get_loiter_time_s())) && + std::fabs(lhs.get_loiter_time_s() - rhs.get_loiter_time_s()) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Loiter time different"; + return false; + } + + if (lhs.get_camera_action() != rhs.get_camera_action()) { + // LogDebug() << "Camera action different"; + return false; + } + + // Underlying is just a float so we can only compare to that accuracy. + if (!(isnan(lhs.get_camera_photo_interval_s()) && isnan(rhs.get_camera_photo_interval_s())) && + float(std::fabs(lhs.get_camera_photo_interval_s() - rhs.get_camera_photo_interval_s())) > + std::numeric_limits::epsilon()) { + // LogDebug() << "Camera photo interval different"; + return false; + } + + return true; } std::ostream &operator<<(std::ostream &str, MissionItem const &mission_item) From 2423a410f4f200faee42488a8a2722b228c2d6af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 23 Jan 2019 14:03:17 +0100 Subject: [PATCH 2/2] mission: use std::isnan() instead of isnan() --- plugins/mission/mission_item.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/plugins/mission/mission_item.cpp b/plugins/mission/mission_item.cpp index b96926c56e..d61bf2aeb3 100644 --- a/plugins/mission/mission_item.cpp +++ b/plugins/mission/mission_item.cpp @@ -133,19 +133,19 @@ bool operator==(const MissionItem &lhs, const MissionItem &rhs) // underlying transport happens with int at 1e7. static constexpr double lat_lon_epsilon = 1e7; - if (!(isnan(lhs.get_latitude_deg()) && isnan(rhs.get_latitude_deg())) && + if (!(std::isnan(lhs.get_latitude_deg()) && std::isnan(rhs.get_latitude_deg())) && std::abs(lhs.get_latitude_deg() - rhs.get_latitude_deg()) > lat_lon_epsilon) { // LogDebug() << "Latitude different"; return false; } - if (!(isnan(lhs.get_longitude_deg()) && isnan(rhs.get_longitude_deg())) && + if (!(std::isnan(lhs.get_longitude_deg()) && std::isnan(rhs.get_longitude_deg())) && std::abs(lhs.get_latitude_deg() - rhs.get_latitude_deg()) > lat_lon_epsilon) { // LogDebug() << "Longitude different"; return false; } - if (!(isnan(lhs.get_relative_altitude_m()) && isnan(rhs.get_relative_altitude_m())) && + if (!(std::isnan(lhs.get_relative_altitude_m()) && std::isnan(rhs.get_relative_altitude_m())) && std::fabs(lhs.get_relative_altitude_m() - rhs.get_relative_altitude_m()) > std::numeric_limits::epsilon()) { // LogDebug() << "Relative altitude different"; @@ -157,28 +157,28 @@ bool operator==(const MissionItem &lhs, const MissionItem &rhs) return false; } - if (!(isnan(lhs.get_speed_m_s()) && isnan(rhs.get_speed_m_s())) && + if (!(std::isnan(lhs.get_speed_m_s()) && std::isnan(rhs.get_speed_m_s())) && std::fabs(lhs.get_speed_m_s() - rhs.get_speed_m_s()) > std::numeric_limits::epsilon()) { // LogDebug() << "Speed different"; return false; } - if (!(isnan(lhs.get_gimbal_pitch_deg()) && isnan(rhs.get_gimbal_pitch_deg())) && + if (!(std::isnan(lhs.get_gimbal_pitch_deg()) && std::isnan(rhs.get_gimbal_pitch_deg())) && std::fabs(lhs.get_gimbal_pitch_deg() - rhs.get_gimbal_pitch_deg()) > std::numeric_limits::epsilon()) { // LogDebug() << "Gimbal pitch different"; return false; } - if (!(isnan(lhs.get_gimbal_yaw_deg()) && isnan(rhs.get_gimbal_yaw_deg())) && + if (!(std::isnan(lhs.get_gimbal_yaw_deg()) && std::isnan(rhs.get_gimbal_yaw_deg())) && std::fabs(lhs.get_gimbal_yaw_deg() - rhs.get_gimbal_yaw_deg()) > std::numeric_limits::epsilon()) { // LogDebug() << "Gimbal yaw different"; return false; } - if (!(isnan(lhs.get_loiter_time_s()) && isnan(rhs.get_loiter_time_s())) && + if (!(std::isnan(lhs.get_loiter_time_s()) && std::isnan(rhs.get_loiter_time_s())) && std::fabs(lhs.get_loiter_time_s() - rhs.get_loiter_time_s()) > std::numeric_limits::epsilon()) { // LogDebug() << "Loiter time different"; @@ -191,7 +191,8 @@ bool operator==(const MissionItem &lhs, const MissionItem &rhs) } // Underlying is just a float so we can only compare to that accuracy. - if (!(isnan(lhs.get_camera_photo_interval_s()) && isnan(rhs.get_camera_photo_interval_s())) && + if (!(std::isnan(lhs.get_camera_photo_interval_s()) && + std::isnan(rhs.get_camera_photo_interval_s())) && float(std::fabs(lhs.get_camera_photo_interval_s() - rhs.get_camera_photo_interval_s())) > std::numeric_limits::epsilon()) { // LogDebug() << "Camera photo interval different";