Skip to content

Commit

Permalink
Add unit tests for gps plugin
Browse files Browse the repository at this point in the history
  • Loading branch information
LukasWoodtli authored and TSC21 committed Jun 5, 2019
1 parent 860e1ce commit c59d214
Show file tree
Hide file tree
Showing 4 changed files with 121 additions and 2 deletions.
4 changes: 3 additions & 1 deletion include/gazebo_gps_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,11 @@ class GAZEBO_VISIBLE GpsPlugin : public ModelPlugin
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
virtual void OnUpdate(const common::UpdateInfo&);

private:
protected:
/* Keep this protected so that it's possible to unit test it. */
std::pair<double, double> reproject(ignition::math::Vector3d& pos);

private:
std::string namespace_;
std::default_random_engine random_generator_;
std::normal_distribution<float> standard_normal_distribution_;
Expand Down
3 changes: 2 additions & 1 deletion src/gazebo_gps_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,8 @@ GpsPlugin::GpsPlugin() : ModelPlugin()

GpsPlugin::~GpsPlugin()
{
updateConnection_->~Connection();
if (updateConnection_)
updateConnection_->~Connection();
}

void GpsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Expand Down
11 changes: 11 additions & 0 deletions unit_tests/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@

if(ENABLE_UNIT_TESTS)

# Gimbal controller plugin
add_executable(gazebo_gimbal_controller_plugin_test gazebo_gimbal_controller_plugin_test.cpp)

target_link_libraries(gazebo_gimbal_controller_plugin_test
Expand All @@ -10,4 +11,14 @@ if(ENABLE_UNIT_TESTS)

add_test(gazebo_gimbal_controller_plugin_test gazebo_gimbal_controller_plugin_test)

# GPS plugin
add_executable(gazebo_gps_plugin_test gazebo_gps_plugin_test.cpp)

target_link_libraries(gazebo_gps_plugin_test
PRIVATE gazebo_gps_plugin
${GTEST_BOTH_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT})

add_test(gazebo_gps_plugin_test gazebo_gps_plugin_test)

endif(ENABLE_UNIT_TESTS)
105 changes: 105 additions & 0 deletions unit_tests/gazebo_gps_plugin_test.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
#include <gtest/gtest.h>

#include "gazebo_gps_plugin.h"


/* Wrap GpsPlugin to get access to protected functions
* for testing. */
class GpsPluginTestHelper : public gazebo::GpsPlugin {
public:
std::pair<double, double> reprojectWrapper(ignition::math::Vector3d& pos) {
return gazebo::GpsPlugin::reproject(pos);
}
};

#define CHECK_RESULT(a, b) do {SCOPED_TRACE("CHECK_RESULT"); checkResult((a),(b));}while(0)
void checkResult(std::pair<double, double> actual, std::pair<double, double> expected) {

ASSERT_DOUBLE_EQ(actual.first, expected.first);
ASSERT_DOUBLE_EQ(actual.second, expected.second);
}

TEST(GPS, TestReproject1) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{0.982794, 1.5708, 0.463648};

std::pair<double, double> expected{0.82724690316405536, 0.14914898037542368};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}


TEST(GPS, TestReproject2) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{0.7833148, 0.1725992, 0.8749541};

std::pair<double, double> expected{0.8272466830789349, 0.14914893398892642};
auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}


TEST(GPS, TestReproject3) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{0.82724671377633008, 0.3676197, 0.5845087};

std::pair<double, double> expected{0.82724671377632109, 0.14914894420476085};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}

TEST(GPS, TestReproject4) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{3.0448264, 2.3968121, 0.2209594};

std::pair<double, double> expected{0.82724703318316262, 0.14914945987599484};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}

TEST(GPS, TestReproject5) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{3.017489057, 0.8586486, 0.3064644};

std::pair<double, double> expected{0.8272467910670781, 0.14914945351884371};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}

TEST(GPS, TestReproject6) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{0.5935629, 3.0111186, 0.8701562};

std::pair<double, double> expected{0.82724712987878102, 0.1491488898645153};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}

TEST(GPS, TestReproject7) {
GpsPluginTestHelper testHelper;

ignition::math::Vector3d pos{0.0658477, 2.7016995, 0.4642607};

std::pair<double, double> expected{0.82724708117437828, 0.14914876715075434};

auto const actual = testHelper.reprojectWrapper(pos);

CHECK_RESULT(actual, expected);
}

0 comments on commit c59d214

Please sign in to comment.