From 37a328070bedd2bc078b20614fe8cf591b1093d9 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Mon, 6 May 2024 16:58:39 +0200 Subject: [PATCH] ControlManager: added transformation services tests --- test/control_manager/CMakeLists.txt | 6 + .../transform_pose_service/CMakeLists.txt | 16 +++ .../transform_pose_service/test.cpp | 103 ++++++++++++++++++ .../transform_pose_service.test | 35 ++++++ .../CMakeLists.txt | 16 +++ .../transform_reference_service/test.cpp | 102 +++++++++++++++++ .../transform_reference_service.test | 35 ++++++ .../transform_vector3_service/CMakeLists.txt | 16 +++ .../transform_vector3_service/test.cpp | 99 +++++++++++++++++ .../transform_vector3_service.test | 35 ++++++ 10 files changed, 463 insertions(+) create mode 100644 test/control_manager/transform_pose_service/CMakeLists.txt create mode 100644 test/control_manager/transform_pose_service/test.cpp create mode 100644 test/control_manager/transform_pose_service/transform_pose_service.test create mode 100644 test/control_manager/transform_reference_service/CMakeLists.txt create mode 100644 test/control_manager/transform_reference_service/test.cpp create mode 100644 test/control_manager/transform_reference_service/transform_reference_service.test create mode 100644 test/control_manager/transform_vector3_service/CMakeLists.txt create mode 100644 test/control_manager/transform_vector3_service/test.cpp create mode 100644 test/control_manager/transform_vector3_service/transform_vector3_service.test diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt index b507b6e..9742b50 100644 --- a/test/control_manager/CMakeLists.txt +++ b/test/control_manager/CMakeLists.txt @@ -41,3 +41,9 @@ add_subdirectory(reference_topic) add_subdirectory(validate_reference_service) add_subdirectory(validate_reference_list_service) + +add_subdirectory(transform_reference_service) + +add_subdirectory(transform_pose_service) + +add_subdirectory(transform_vector3_service) diff --git a/test/control_manager/transform_pose_service/CMakeLists.txt b/test/control_manager/transform_pose_service/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/control_manager/transform_pose_service/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/control_manager/transform_pose_service/test.cpp b/test/control_manager/transform_pose_service/test.cpp new file mode 100644 index 0000000..44a59c0 --- /dev/null +++ b/test/control_manager/transform_pose_service/test.cpp @@ -0,0 +1,103 @@ +#include + +#include + +using radians = mrs_lib::geometry::radians; +using sradians = mrs_lib::geometry::sradians; + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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(); + } + + { + auto [success, message] = uh->activateMidAir(); + + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + { + std::string target_frame = _uav_name_ + "/world_origin"; + + geometry_msgs::PoseStamped msg; + + msg.header.frame_id = _uav_name_ + "/fcu"; + msg.pose.position.x = 10; + msg.pose.position.y = 1; + msg.pose.position.z = 2; + msg.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(2); + + auto gt_tfed_pose = uh->transformer_->transformSingle(msg, target_frame); + + if (!gt_tfed_pose) { + ROS_ERROR("[%s]: failed to transform the pose", ros::this_node::getName().c_str()); + return false; + } + + auto [success, message, pose_tfed] = uh->transformPose(msg, target_frame); + + if (!success) { + ROS_ERROR("[%s]: pose #1 transformation failed: '%s'", ros::this_node::getName().c_str(), message->c_str()); + return false; + } + + if (std::abs(gt_tfed_pose->pose.position.x - pose_tfed->pose.position.x) > 0.1 || + std::abs(gt_tfed_pose->pose.position.y - pose_tfed->pose.position.y) > 0.1 || + std::abs(gt_tfed_pose->pose.position.z - pose_tfed->pose.position.z) > 0.1 || + std::abs(sradians::diff(mrs_lib::AttitudeConverter(gt_tfed_pose->pose.orientation).getHeading(), + mrs_lib::AttitudeConverter(pose_tfed->pose.orientation).getHeading())) > 0.1) { + + ROS_ERROR("[%s]: pose #1 transformation failed, the poses don't match", ros::this_node::getName().c_str()); + return false; + } + } + + if (uh->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/test/control_manager/transform_pose_service/transform_pose_service.test b/test/control_manager/transform_pose_service/transform_pose_service.test new file mode 100644 index 0000000..604b743 --- /dev/null +++ b/test/control_manager/transform_pose_service/transform_pose_service.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/transform_reference_service/CMakeLists.txt b/test/control_manager/transform_reference_service/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/control_manager/transform_reference_service/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/control_manager/transform_reference_service/test.cpp b/test/control_manager/transform_reference_service/test.cpp new file mode 100644 index 0000000..8776d2c --- /dev/null +++ b/test/control_manager/transform_reference_service/test.cpp @@ -0,0 +1,102 @@ +#include + +#include + +using radians = mrs_lib::geometry::radians; +using sradians = mrs_lib::geometry::sradians; + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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(); + } + + { + auto [success, message] = uh->activateMidAir(); + + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + { + std::string target_frame = _uav_name_ + "/world_origin"; + + mrs_msgs::ReferenceStamped msg; + + msg.header.frame_id = _uav_name_ + "/fcu"; + msg.reference.position.x = 10; + msg.reference.position.y = 1; + msg.reference.position.z = 2; + msg.reference.heading = 2; + + auto gt_tfed_reference = uh->transformer_->transformSingle(msg, target_frame); + + if (!gt_tfed_reference) { + ROS_ERROR("[%s]: failed to transform the reference", ros::this_node::getName().c_str()); + return false; + } + + auto [success, message, ref_tfed] = uh->transformReference(msg, target_frame); + + if (!success) { + ROS_ERROR("[%s]: reference #1 transformation failed: '%s'", ros::this_node::getName().c_str(), message->c_str()); + return false; + } + + if (std::abs(gt_tfed_reference->reference.position.x - ref_tfed->reference.position.x) > 0.1 || + std::abs(gt_tfed_reference->reference.position.y - ref_tfed->reference.position.y) > 0.1 || + std::abs(gt_tfed_reference->reference.position.z - ref_tfed->reference.position.z) > 0.1 || + std::abs(sradians::diff(gt_tfed_reference->reference.heading, ref_tfed->reference.heading)) > 0.1) { + + ROS_ERROR("[%s]: reference #1 transformation failed, the references don't match", ros::this_node::getName().c_str()); + return false; + } + } + + if (uh->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/test/control_manager/transform_reference_service/transform_reference_service.test b/test/control_manager/transform_reference_service/transform_reference_service.test new file mode 100644 index 0000000..604b743 --- /dev/null +++ b/test/control_manager/transform_reference_service/transform_reference_service.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/transform_vector3_service/CMakeLists.txt b/test/control_manager/transform_vector3_service/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/control_manager/transform_vector3_service/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/control_manager/transform_vector3_service/test.cpp b/test/control_manager/transform_vector3_service/test.cpp new file mode 100644 index 0000000..a709d54 --- /dev/null +++ b/test/control_manager/transform_vector3_service/test.cpp @@ -0,0 +1,99 @@ +#include + +#include + +using radians = mrs_lib::geometry::radians; +using sradians = mrs_lib::geometry::sradians; + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + bool test(); +}; + +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(); + } + + { + auto [success, message] = uh->activateMidAir(); + + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + { + std::string target_frame = _uav_name_ + "/world_origin"; + + geometry_msgs::Vector3Stamped msg; + + msg.header.frame_id = _uav_name_ + "/fcu"; + msg.vector.x = 1; + msg.vector.y = 2; + msg.vector.z = 3; + + auto gt_tfed_vector = uh->transformer_->transformSingle(msg, target_frame); + + if (!gt_tfed_vector) { + ROS_ERROR("[%s]: failed to transform the vector", ros::this_node::getName().c_str()); + return false; + } + + auto [success, message, vec_tfed] = uh->transformVector3(msg, target_frame); + + if (!success) { + ROS_ERROR("[%s]: vector #1 transformation failed: '%s'", ros::this_node::getName().c_str(), message->c_str()); + return false; + } + + if (std::abs(gt_tfed_vector->vector.x - vec_tfed->vector.x) > 0.1 || std::abs(gt_tfed_vector->vector.y - vec_tfed->vector.y) > 0.1 || + std::abs(gt_tfed_vector->vector.z - vec_tfed->vector.z) > 0.1) { + + ROS_ERROR("[%s]: vector #1 transformation failed, the vectors don't match", ros::this_node::getName().c_str()); + return false; + } + } + + if (uh->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/test/control_manager/transform_vector3_service/transform_vector3_service.test b/test/control_manager/transform_vector3_service/transform_vector3_service.test new file mode 100644 index 0000000..604b743 --- /dev/null +++ b/test/control_manager/transform_vector3_service/transform_vector3_service.test @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +