diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt index 5084ddf..51372ef 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt +++ b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt @@ -1,3 +1,5 @@ find_package(rostest REQUIRED) add_subdirectory(takeoff) + +add_subdirectory(uav_models) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/CMakeLists.txt new file mode 100644 index 0000000..d3144cd --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/CMakeLists.txt @@ -0,0 +1,33 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + +set(MODELS + x500 + f330 + f450 + f550 + t650 + m690 + naki + ) + +catkin_add_executable_with_gtest(test_${TEST_NAME} + test.cpp + ) + +target_link_libraries(test_${TEST_NAME} + ${catkin_LIBRARIES} + ) + +add_dependencies(test_${TEST_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +foreach(MODEL ${MODELS}) + + add_rostest(${TEST_NAME}.test + ARGS + UAV_TYPE:=${MODEL} + ) + +endforeach() diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/config/custom_config.yaml new file mode 100644 index 0000000..4b0275a --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/config/custom_config.yaml @@ -0,0 +1,16 @@ +# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: +## -------------------------------------------------------------- +## | rosrun mrs_uav_core get_public_params.py # +## -------------------------------------------------------------- + +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + ] + + initial_state_estimator: "gps_garmin" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp new file mode 100644 index 0000000..6eb2ab8 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/test.cpp @@ -0,0 +1,62 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +bool Tester::test() { + + { + auto [success, message] = spawnGazeboUav(); + + if (!success) { + ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + { + auto [success, message] = takeoff(); + + if (!success) { + ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + this->sleep(5.0); + + if (this->isFlyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; + } +} + + +TEST(TESTSuite, test) { + + Tester tester; + + bool result = tester.test(); + + if (result) { + GTEST_SUCCEED(); + } else { + GTEST_FAIL(); + } +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "test"); + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/uav_models.test b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/uav_models.test new file mode 100644 index 0000000..ba16e3a --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/uav_models/uav_models.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +