ControlManager: added transformation services tests

ctu-mrs · May 6, 2024 · 37a3280 · 37a3280
1 parent 2a2a0ad
commit 37a3280
Show file tree

Hide file tree

Showing 10 changed files with 463 additions and 0 deletions.
diff --git 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
@@ -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
@@ -0,0 +1,103 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+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<mrs_uav_testing::UAVHandler> 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
@@ -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>
diff --git a/test/control_manager/transform_reference_service/CMakeLists.txt 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
@@ -0,0 +1,102 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+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<mrs_uav_testing::UAVHandler> 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
@@ -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>
diff --git a/test/control_manager/transform_vector3_service/CMakeLists.txt 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)