Skip to content

Commit

Permalink
added eland service test
Browse files Browse the repository at this point in the history
  • Loading branch information
klaxalk committed Dec 19, 2023
1 parent 4a7cb9c commit d7f3153
Show file tree
Hide file tree
Showing 4 changed files with 172 additions and 0 deletions.
2 changes: 2 additions & 0 deletions test/control_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,5 @@ add_subdirectory(eland_control_error)
add_subdirectory(eland_innovation)

add_subdirectory(failsafe_control_error)

add_subdirectory(eland_service)
16 changes: 16 additions & 0 deletions test/control_manager/eland_service/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)

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}
)

add_rostest(${TEST_NAME}.test)
35 changes: 35 additions & 0 deletions test/control_manager/eland_service/eland_service.test
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
<launch>

<arg name="this_path" default="$(dirname)" />

<arg name="UAV_NAME" default="uav1" />
<arg name="UAV_TYPE" default="x500" />

<!-- automatically deduce the test name -->
<arg name="test_name" default="$(eval arg('this_path').split('/')[-1])" />

<!-- automatically deduce the package name -->
<arg name="import_eval" default="eval('_' + '_import_' + '_')"/>
<arg name="package_eval" default="eval(arg('import_eval') + '(\'rospkg\')').get_package_name(arg('this_path'))" />
<arg name="package" default="$(eval eval(arg('package_eval')))" />

<include file="$(find mrs_uav_testing)/launch/mrs_simulator.launch">
<arg name="UAV_NAME" default="$(arg UAV_NAME)" />
</include>

<include file="$(find mrs_multirotor_simulator)/launch/hw_api.launch">
<arg name="UAV_NAME" default="$(arg UAV_NAME)" />
</include>

<include file="$(find mrs_uav_testing)/launch/mrs_uav_system.launch">
<arg name="automatic_start" default="false" />
<arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
<arg name="UAV_NAME" default="$(arg UAV_NAME)" />
</include>

<test pkg="$(arg package)" type="test_$(arg test_name)" test-name="$(arg test_name)" time-limit="60.0">
<param name="test" value="$(arg test_name)" />
<param name="uav_name" value="$(arg UAV_NAME)" />
</test>

</launch>
119 changes: 119 additions & 0 deletions test/control_manager/eland_service/test.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,119 @@
#include <gtest/gtest.h>

#include <mrs_uav_testing/test_generic.h>

/* class Tester //{ */

class Tester : public mrs_uav_testing::TestGeneric {

public:
bool test();

Tester();

mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;

std::tuple<bool, std::string> eland();
};

Tester::Tester() {

sch_eland_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/control_manager/eland");
}

std::tuple<bool, std::string> Tester::eland() {

{
std_srvs::Trigger srv;

{
bool service_call = sch_eland_.call(srv);

if (!service_call || !srv.response.success) {
return {false, "offboard service call failed"};
}
}
}

return {true, "service called"};
}

//}

bool Tester::test() {

{
auto [success, message] = activateMidAir();

if (!success) {
ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
return false;
}
}

// | ---------------------- trigger eland --------------------- |

{
auto [success, message] = eland();

if (!success) {
ROS_ERROR("[%s]: eland call failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
return false;
}
}

// | -------------- wait for the eland to trigger ------------- |

while (true) {

if (!ros::ok()) {
return false;
}

if (!isFlyingNormally() && getActiveController() == "EmergencyController" && getActiveTracker() == "LandoffTracker") {
break;
}

sleep(0.01);
}

// | -------------------- wait for landing -------------------- |

while (true) {

if (!ros::ok()) {
return false;
}

if (!this->isOutputEnabled()) {
return true;
}

sleep(0.01);
}

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();
}

0 comments on commit d7f3153

Please sign in to comment.