From a5ac4fa026d9f41470ce5796f00deb08125a6c34 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 16 Apr 2024 08:53:10 +0200 Subject: [PATCH] fxd land_home bug with moving takeoff reference frame --- src/uav_manager.cpp | 2 + test/uav_manager/CMakeLists.txt | 2 + .../CMakeLists.txt | 16 ++ .../config/automatic_start.yaml | 6 + .../config/custom_config.yaml | 15 ++ .../config/mrs_simulator.yaml | 11 ++ .../land_home_moving_takeoff_frame.test | 38 ++++ .../land_home_moving_takeoff_frame/test.cpp | 163 ++++++++++++++++++ 8 files changed, 253 insertions(+) create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/CMakeLists.txt create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/config/automatic_start.yaml create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/config/custom_config.yaml create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/config/mrs_simulator.yaml create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test create mode 100644 test/uav_manager/land_home_moving_takeoff_frame/test.cpp diff --git a/src/uav_manager.cpp b/src/uav_manager.cpp index 7120c1f..4f839db 100644 --- a/src/uav_manager.cpp +++ b/src/uav_manager.cpp @@ -1752,6 +1752,8 @@ bool UavManager::callbackLandHome([[maybe_unused]] std_srvs::Trigger::Request& r reference_out.header.frame_id = land_there_reference_.header.frame_id; reference_out.header.stamp = ros::Time::now(); reference_out.reference = land_there_reference_.reference; + + land_there_reference_ = reference_out; } bool service_success = emergencyReferenceSrv(reference_out); diff --git a/test/uav_manager/CMakeLists.txt b/test/uav_manager/CMakeLists.txt index 063407f..9c9df48 100644 --- a/test/uav_manager/CMakeLists.txt +++ b/test/uav_manager/CMakeLists.txt @@ -12,6 +12,8 @@ add_subdirectory(landing) add_subdirectory(land_home) +add_subdirectory(land_home_moving_takeoff_frame) + add_subdirectory(land_there) add_subdirectory(min_height_check) diff --git a/test/uav_manager/land_home_moving_takeoff_frame/CMakeLists.txt b/test/uav_manager/land_home_moving_takeoff_frame/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/CMakeLists.txt @@ -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) diff --git a/test/uav_manager/land_home_moving_takeoff_frame/config/automatic_start.yaml b/test/uav_manager/land_home_moving_takeoff_frame/config/automatic_start.yaml new file mode 100644 index 0000000..81a08b8 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/config/automatic_start.yaml @@ -0,0 +1,6 @@ +preflight_check: + + # takeoff not allowed if UAV's height sensor exceeds + height_check: + + enabled: false diff --git a/test/uav_manager/land_home_moving_takeoff_frame/config/custom_config.yaml b/test/uav_manager/land_home_moving_takeoff_frame/config/custom_config.yaml new file mode 100644 index 0000000..b36ae76 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/config/custom_config.yaml @@ -0,0 +1,15 @@ +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, ground_truth, dummy + state_estimators: [ + "gps_garmin", + "gps_baro", + # "rtk", + # "ground_truth", + # "dummy" + ] + + initial_state_estimator: "gps_garmin" # will be used as the first state estimator diff --git a/test/uav_manager/land_home_moving_takeoff_frame/config/mrs_simulator.yaml b/test/uav_manager/land_home_moving_takeoff_frame/config/mrs_simulator.yaml new file mode 100644 index 0000000..57c2b86 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/config/mrs_simulator.yaml @@ -0,0 +1,11 @@ +# turning off for the takeoff test +individual_takeoff_platform: + enabled: true + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 20.0 + z: 3.0 + heading: 1.2 diff --git a/test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test b/test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test new file mode 100644 index 0000000..ddd1855 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/uav_manager/land_home_moving_takeoff_frame/test.cpp b/test/uav_manager/land_home_moving_takeoff_frame/test.cpp new file mode 100644 index 0000000..f1fe779 --- /dev/null +++ b/test/uav_manager/land_home_moving_takeoff_frame/test.cpp @@ -0,0 +1,163 @@ +#include + +#include + +#include + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + Tester(); + + bool asyncSetGroundZ(); + + mrs_lib::ServiceClientHandler sch_set_ground_z_; +}; + +Tester::Tester() { + + sch_set_ground_z_ = mrs_lib::ServiceClientHandler(nh_, "/multirotor_simulator/uav1/set_ground_z"); +} + +bool Tester::test() { + + std::shared_ptr uh; + + { + auto [uhopt, message] = getUAVHandler(_uav_name_); + + if (!uhopt) { + ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str()); + return false; + } + + uh = uhopt.value(); + } + + // | ------------- wait for the system to be ready ------------ | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (uh->mrsSystemReady()) { + break; + } + } + + // | ---------------- save the current position --------------- | + + auto takeoff_pos = uh->sh_uav_state_.getMsg()->pose.position; + auto takeoff_hdg = mrs_lib::AttitudeConverter(uh->sh_uav_state_.getMsg()->pose.orientation).getHeading(); + + // | ------------------------ take off ------------------------ | + + { + auto [success, message] = uh->takeoff(); + + if (!success) { + ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(1.0); + + // | --------------------- goto somewhere --------------------- | + + { + auto [success, message] = uh->gotoRel(10, 0, 0, 0); + + if (!success) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(1.0); + + // | -------------------- switch estimator -------------------- | + + { + auto [success, message] = uh->switchEstimator("gps_baro"); + + if (!success) { + ROS_ERROR("[%s]: failed to switch the estimator gps_baro, message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + sleep(1.0); + + // | -------- run async task for setting ground z later ------- | + + auto future_res = std::async(std::launch::async, &Tester::asyncSetGroundZ, this); + + // | ------------------------ land home ----------------------- | + + { + auto [success, message] = uh->landHome(); + + if (!success) { + ROS_ERROR("[%s]: land home failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + if (!future_res.valid() || !future_res.get()) { + ROS_ERROR("[%s]: was not able to set ground z", ros::this_node::getName().c_str()); + return false; + } + + // | ---------------- check the final position ---------------- | + + if (uh->isAtPosition(takeoff_pos.x, takeoff_pos.y, takeoff_hdg, 0.5)) { + return true; + } else { + ROS_ERROR("[%s]: land home did end in wrong place", ros::this_node::getName().c_str()); + return false; + } +} + +bool Tester::asyncSetGroundZ() { + + this->sleep(1.5); + + mrs_msgs::Float64Srv srv; + srv.request.value = -1.0; + + bool service_call = sch_set_ground_z_.call(srv); + + if (!service_call || !srv.response.success) { + ROS_ERROR("[%s]: failed to call the service for setting ground z", ros::this_node::getName().c_str()); + return false; + } else { + return true; + } +} + +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(); +}