From 3323736fca292ea55f72fa86f4639557db60aae1 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Wed, 6 Dec 2023 10:14:25 +0100 Subject: [PATCH] added failsafe tests --- CMakeLists.txt | 1 + package.xml | 1 + test/CMakeLists.txt | 5 +- test/all_tests.sh | 12 +- test/coverage.sh | 4 +- test/failsafe_controller/CMakeLists.txt | 37 ++++ .../hw_api/hw_api_acceleration_hdg.yaml | 10 + .../hw_api/hw_api_acceleration_hdg_rate.yaml | 10 + .../config/hw_api/hw_api_actuators.yaml | 10 + .../config/hw_api/hw_api_attitude.yaml | 10 + .../config/hw_api/hw_api_attitude_rate.yaml | 10 + .../config/hw_api/hw_api_control_group.yaml | 10 + .../config/hw_api/hw_api_position.yaml | 10 + .../config/hw_api/hw_api_velocity_hdg.yaml | 10 + .../hw_api/hw_api_velocity_hdg_rate.yaml | 10 + .../failsafe_controller.test | 35 ++++ test/failsafe_controller/test.cpp | 109 ++++++++++ test/modalities/CMakeLists.txt | 16 +- test/modalities/config/network_config.yaml | 15 -- test/modalities/config/simulator.yaml | 20 -- test/modalities/config/world_config.yaml | 34 --- test/modalities/launch/mrs_uav_system.launch | 34 --- test/modalities/modalities.test | 33 ++- test/modalities/test.cpp | 197 ++---------------- 24 files changed, 342 insertions(+), 301 deletions(-) create mode 100644 test/failsafe_controller/CMakeLists.txt create mode 100644 test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg_rate.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_actuators.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_attitude.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_attitude_rate.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_control_group.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_position.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_velocity_hdg.yaml create mode 100644 test/failsafe_controller/config/hw_api/hw_api_velocity_hdg_rate.yaml create mode 100644 test/failsafe_controller/failsafe_controller.test create mode 100644 test/failsafe_controller/test.cpp delete mode 100644 test/modalities/config/network_config.yaml delete mode 100644 test/modalities/config/simulator.yaml delete mode 100644 test/modalities/config/world_config.yaml delete mode 100644 test/modalities/launch/mrs_uav_system.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index 4007c9e..c9908a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ set(CATKIN_DEPENDENCIES pluginlib roscpp rospy + mrs_uav_testing ) find_package(catkin REQUIRED COMPONENTS diff --git a/package.xml b/package.xml index bd43e04..38dd667 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ pluginlib roscpp rospy + mrs_uav_testing diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 3f1be7c..3df80f0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,3 +1,6 @@ find_package(rostest REQUIRED) +find_package(mrs_uav_testing REQUIRED) -add_subdirectory(modalities) +# add_subdirectory(modalities) + +add_subdirectory(failsafe_controller) diff --git a/test/all_tests.sh b/test/all_tests.sh index 42ff4a7..aa6ca4e 100755 --- a/test/all_tests.sh +++ b/test/all_tests.sh @@ -5,14 +5,8 @@ set -e trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR -PACKAGE="mrs_uav_controllers" -VERBOSE=1 - -[ "$VERBOSE" = "0" ] && TEXT_OUTPUT="" -[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t" - # build the package -catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests -catkin build $PACKAGE --no-deps --catkin-make-args tests +catkin build --this # it has to be fully built normally before building with --catkin-make-args tests +catkin build --this --no-deps --catkin-make-args tests -catkin test --this -i +catkin test --this -i -p 1 -s diff --git a/test/coverage.sh b/test/coverage.sh index 0c2f9c0..d7b2ef7 100755 --- a/test/coverage.sh +++ b/test/coverage.sh @@ -15,8 +15,8 @@ cd build lcov --capture --directory . --output-file coverage.info lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed -lcov --extract coverage.info.removed "*/src/*${PACKAGE_NAME}/*" --output-file coverage.info.cleaned -genhtml --title "${PACKAGE_NAME} - Test coverage report" --demangle-cpp --legend --frames --show-details -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log +lcov --extract coverage.info.removed "*/mrs_uav_autostart/*" --output-file coverage.info.cleaned +genhtml -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log COVERAGE_PCT=`cat /tmp/genhtml.log | tail -n 1 | awk '{print $2}'` diff --git a/test/failsafe_controller/CMakeLists.txt b/test/failsafe_controller/CMakeLists.txt new file mode 100644 index 0000000..62af09f --- /dev/null +++ b/test/failsafe_controller/CMakeLists.txt @@ -0,0 +1,37 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + +set(MODALITIES + actuators + control_group + attitude_rate + attitude + # acceleration_hdg_rate + # acceleration_hdg + # velocity_hdg_rate + # velocity_hdg + # position + ) + +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(MODALITY ${MODALITIES}) + + add_rostest(${TEST_NAME}.test + ARGS + package_name:=${PROJECT_NAME} + test_name:=${TEST_NAME} + modality:=${MODALITY} + ) + +endforeach() diff --git a/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg.yaml b/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg.yaml new file mode 100644 index 0000000..616d1cc --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: true + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg_rate.yaml b/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg_rate.yaml new file mode 100644 index 0000000..03b695d --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_acceleration_hdg_rate.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: true + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_actuators.yaml b/test/failsafe_controller/config/hw_api/hw_api_actuators.yaml new file mode 100644 index 0000000..7e3bbed --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_actuators.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: true + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_attitude.yaml b/test/failsafe_controller/config/hw_api/hw_api_attitude.yaml new file mode 100644 index 0000000..7bc7783 --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_attitude.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_attitude_rate.yaml b/test/failsafe_controller/config/hw_api/hw_api_attitude_rate.yaml new file mode 100644 index 0000000..0095b3e --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_attitude_rate.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: true + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_control_group.yaml b/test/failsafe_controller/config/hw_api/hw_api_control_group.yaml new file mode 100644 index 0000000..3b9fc92 --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_control_group.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: true + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_position.yaml b/test/failsafe_controller/config/hw_api/hw_api_position.yaml new file mode 100644 index 0000000..a9ac433 --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_position.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: true diff --git a/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg.yaml b/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg.yaml new file mode 100644 index 0000000..ec85886 --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: true + position: false diff --git a/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg_rate.yaml b/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg_rate.yaml new file mode 100644 index 0000000..2ed1dc0 --- /dev/null +++ b/test/failsafe_controller/config/hw_api/hw_api_velocity_hdg_rate.yaml @@ -0,0 +1,10 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: false + attitude: false + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: true + velocity_hdg: false + position: false diff --git a/test/failsafe_controller/failsafe_controller.test b/test/failsafe_controller/failsafe_controller.test new file mode 100644 index 0000000..5b25da4 --- /dev/null +++ b/test/failsafe_controller/failsafe_controller.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/failsafe_controller/test.cpp b/test/failsafe_controller/test.cpp new file mode 100644 index 0000000..9795276 --- /dev/null +++ b/test/failsafe_controller/test.cpp @@ -0,0 +1,109 @@ +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + Tester(); + + mrs_lib::ServiceClientHandler sch_failsafe_; + + std::string _modality_; +}; + +Tester::Tester() : mrs_uav_testing::TestGeneric() { + + pl_->loadParam("modality", _modality_, std::string()); + + sch_failsafe_ = mrs_lib::ServiceClientHandler(nh_, "/" + _uav_name_ + "/control_manager/failsafe"); +} + +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 failsafe -------------------- | + + { + std_srvs::Trigger srv; + + { + bool service_call = sch_failsafe_.call(srv); + + if (!service_call || !srv.response.success) { + return false; + } + } + } + + // | -------- wait till the right controller is active -------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (_modality_ == "position") { + if (sh_control_manager_diag_.getMsg()->active_controller == "EmergencyController") { + break; + } + } else { + if (sh_control_manager_diag_.getMsg()->active_controller == "FailsafeController") { + break; + } + } + + sleep(0.1); + } + + // | ----------------- wait till the UAV lands ---------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!isOutputEnabled()) { + return true; + } + + sleep(0.1); + } + + 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) { + + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "test"); + + return RUN_ALL_TESTS(); +} diff --git a/test/modalities/CMakeLists.txt b/test/modalities/CMakeLists.txt index f6fa819..022a654 100644 --- a/test/modalities/CMakeLists.txt +++ b/test/modalities/CMakeLists.txt @@ -1,3 +1,5 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + set(MODALITIES actuators control_group @@ -15,15 +17,15 @@ set(CONTROLLERS mpc_controller ) -catkin_add_executable_with_gtest(test_modalities +catkin_add_executable_with_gtest(test_${TEST_NAME} test.cpp ) -target_link_libraries(test_modalities +target_link_libraries(test_${TEST_NAME} ${catkin_LIBRARIES} ) -add_dependencies(test_modalities +add_dependencies(test_${TEST_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) @@ -32,7 +34,13 @@ foreach(CONTROLLER ${CONTROLLERS}) foreach(MODALITY ${MODALITIES}) - add_rostest(modalities.test ARGS controller:=${CONTROLLER} modality:=${MODALITY}) + add_rostest(${TEST_NAME}.test + ARGS + package_name:=${PROJECT_NAME} + test_name:=${TEST_NAME} + controller:=${CONTROLLER} + modality:=${MODALITY} + ) endforeach() diff --git a/test/modalities/config/network_config.yaml b/test/modalities/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/test/modalities/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/test/modalities/config/simulator.yaml b/test/modalities/config/simulator.yaml deleted file mode 100644 index 196c2e8..0000000 --- a/test/modalities/config/simulator.yaml +++ /dev/null @@ -1,20 +0,0 @@ -simulation_rate: 250.0 -clock_rate: 250.0 -realtime_factor: 1.0 - -iterate_without_input: true - -individual_takeoff_platform: - enabled: true - -uav_names: [ - "uav1", -] - -uav1: - type: "x500" - spawn: - x: 10.0 - y: 15.0 - z: 2.0 - heading: 0 diff --git a/test/modalities/config/world_config.yaml b/test/modalities/config/world_config.yaml deleted file mode 100644 index 9b55067..0000000 --- a/test/modalities/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: true - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/test/modalities/launch/mrs_uav_system.launch b/test/modalities/launch/mrs_uav_system.launch deleted file mode 100644 index be8ca11..0000000 --- a/test/modalities/launch/mrs_uav_system.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/modalities/modalities.test b/test/modalities/modalities.test index e1fe89b..9e5358c 100644 --- a/test/modalities/modalities.test +++ b/test/modalities/modalities.test @@ -1,13 +1,36 @@ - + + + + + + + + + + + + + + + + + + + - - - + + + + + - + + + + diff --git a/test/modalities/test.cpp b/test/modalities/test.cpp index 04bd7c3..c463a94 100644 --- a/test/modalities/test.cpp +++ b/test/modalities/test.cpp @@ -1,202 +1,49 @@ #include -#include -#include -#include -#include +#include -#include -#include -#include -#include - -#include -#include - -/* class Tester //{ */ - -class Tester { +class Tester : public mrs_uav_testing::TestGeneric { public: - Tester(); - bool test(); - -private: - ros::NodeHandle nh_; - std::shared_ptr spinner_; - - mrs_lib::SubscribeHandler sh_control_manager_diag_; - mrs_lib::SubscribeHandler sh_estim_manager_diag_; - - mrs_lib::ServiceClientHandler sch_arming_; - mrs_lib::ServiceClientHandler sch_midair_; - - mrs_lib::ServiceClientHandler sch_goto_; }; -//} - -/* Tester() //{ */ - -Tester::Tester() { - - // | ------------------ initialize test node ------------------ | - - nh_ = ros::NodeHandle("~"); - - ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); - - ros::Time::waitForValid(); - - spinner_ = std::make_shared(4); - spinner_->start(); - - std::string uav_name = "uav1"; - - // | ----------------------- subscribers ---------------------- | - - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "Test"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - - sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); - sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); - - ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); - - // | --------------------- service clients -------------------- | - - sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); - sch_midair_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/uav_manager/midair_activation"); - - sch_goto_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/control_manager/goto"); - - ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); -} - -//} - -/* test() //{ */ - bool Tester::test() { - // | ---------------- wait for ready to takeoff --------------- | - - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); - - if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { - break; - } - - ros::Duration(0.01).sleep(); - } - - ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); - - ros::Duration(1.0).sleep(); - - // | ---------------------- arm the drone --------------------- | - - ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); - { - std_srvs::SetBool arming; - arming.request.data = true; + auto [success, message] = activateMidAir(); - { - bool service_call = sch_arming_.call(arming); - - if (!service_call || !arming.response.success) { - ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); - return false; - } + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; } } - // | -------------------------- wait -------------------------- | - - ros::Duration(0.2).sleep(); - - // | -------------------- midair activation ------------------- | - - ROS_INFO("[%s]: activating the drone 'in mid air'", ros::this_node::getName().c_str()); - { - std_srvs::Trigger trigger; + auto [success, message] = this->gotoAbs(0, 0, 2.0, 0); - { - bool service_call = sch_midair_.call(trigger); - - if (!service_call || !trigger.response.success) { - ROS_ERROR("[%s]: midair activation service call failed", ros::this_node::getName().c_str()); - return false; - } - } - } - - // | ----------------- sleep before the action ---------------- | - - ros::Duration(1.0).sleep(); - - // | ------------------------ test goto ----------------------- | - - mrs_msgs::Vec4 goto_cmd; - - { - goto_cmd.request.goal[0] = 0; - goto_cmd.request.goal[1] = 0; - goto_cmd.request.goal[2] = 5.5; - goto_cmd.request.goal[3] = 2.2; - - ROS_INFO("[%s]: calling goto", ros::this_node::getName().c_str()); - - { - bool service_call = sch_goto_.call(goto_cmd); - - if (!service_call || !goto_cmd.response.success) { - ROS_ERROR("[%s]: goto service call failed", ros::this_node::getName().c_str()); - return false; - } + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; } } - ros::Duration(1.0).sleep(); - - // | ------------------- check if we are ok ------------------- | + this->sleep(5.0); - while (ros::ok()) { - - ROS_INFO_THROTTLE(1.0, "[%s]: checking if we are still flying", ros::this_node::getName().c_str()); - - if (!sh_control_manager_diag_.getMsg()->flying_normally) { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } - - if (!sh_control_manager_diag_.getMsg()->tracker_status.have_goal) { - ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); - return true; - } + if (this->isFlyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); + return false; } - - return false; } -//} -std::shared_ptr tester_; +TEST(TESTSuite, test) { -TEST(TESTSuite, Se3Controller_modalities) { + Tester tester; - bool result = tester_->test(); + bool result = tester.test(); if (result) { GTEST_SUCCEED(); @@ -207,13 +54,9 @@ TEST(TESTSuite, Se3Controller_modalities) { int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { - ros::init(argc, argv, "se3controller_modalities"); - - tester_ = std::make_shared(); + ros::init(argc, argv, "test"); testing::InitGoogleTest(&argc, argv); - Tester tester; - return RUN_ALL_TESTS(); }