Skip to content

Commit

Permalink
Merge pull request #650 from Dronecode/fix-mission-item-comparison
Browse files Browse the repository at this point in the history 
mission: make mission item comparison less strict
  • Loading branch information
@julianoes
julianoes committed Jan 23, 2019
2 parents fdb09a7 + 2423a41 commit 89cbac7
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 38 deletions.
22 changes: 1 addition & 21 deletions integration_tests/mission.cpp
View file
Expand Up @@ -262,27 +262,7 @@ std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
void compare_mission_items(const std::shared_ptr<MissionItem> original,
const std::shared_ptr<MissionItem> 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> mission)
Expand Down
90 changes: 73 additions & 17 deletions plugins/mission/mission_item.cpp
View file
Expand Up @@ -3,6 +3,8 @@
#include <iomanip>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>

#include "mission_item_impl.h"

Expand Down Expand Up @@ -127,23 +129,77 @@ 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 (!(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 (!(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 (!(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<float>::epsilon()) {
// LogDebug() << "Relative altitude different";
return false;
}

if (lhs.get_fly_through() != rhs.get_fly_through()) {
// LogDebug() << "Fly-through different."
return false;
}

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<float>::epsilon()) {
// LogDebug() << "Speed different";
return false;
}

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<float>::epsilon()) {
// LogDebug() << "Gimbal pitch different";
return false;
}

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<float>::epsilon()) {
// LogDebug() << "Gimbal yaw different";
return false;
}

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<float>::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 (!(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<float>::epsilon()) {
// LogDebug() << "Camera photo interval different";
return false;
}

return true;
}

std::ostream &operator<<(std::ostream &str, MissionItem const &mission_item)
Expand Down

0 comments on commit 89cbac7

Please sign in to comment.