added test for before takeoff path setting

test/CMakeLists.txt

@@ -5,3 +5,5 @@ add_subdirectory(service_fly_now)
 add_subdirectory(topic_fly_now)
 
 add_subdirectory(fallback_sampling)
+
+add_subdirectory(path_before_takeoff)

test/path_before_takeoff/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)

test/path_before_takeoff/config/mrs_simulator.yaml

@@ -0,0 +1,14 @@
+individual_takeoff_platform:
+  enabled: false
+
+uav_names: [
+  "uav1",
+]
+
+uav1:
+  type: "x500"
+  spawn:
+    x: 10.0
+    y: 20.0
+    z: 0.0
+    heading: 1.2

test/path_before_takeoff/path_before_takeoff.test

@@ -0,0 +1,36 @@
+<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="true" />
+    <arg name="platform_config" default="$(find mrs_multirotor_simulator)/config/mrs_uav_system/$(arg UAV_TYPE).yaml" />
+    <arg name="custom_config" default="$(dirname)/config/custom_config.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="120.0">
+    <param name="test" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+  </test>
+
+</launch>

test/path_before_takeoff/test.cpp

@@ -0,0 +1,149 @@
+#include <gtest/gtest.h>
+
+// include the generic test customized for this package
+#include <trajectory_generation_test.h>
+
+class Tester : public TrajectoryGenerationTest {
+
+public:
+  bool test();
+};
+
+bool Tester::test() {
+
+  std::vector<Eigen::Vector4d> points;
+
+  points.push_back(Eigen::Vector4d(-5, -5, 5, 1));
+  points.push_back(Eigen::Vector4d(-5, 5, 5, 2));
+  points.push_back(Eigen::Vector4d(5, -5, 5, 3));
+  points.push_back(Eigen::Vector4d(5, 5, 5, 4));
+
+  // | ---------------- prepare the path message ---------------- |
+
+  mrs_msgs::Path path;
+
+  path.fly_now     = true;
+  path.use_heading = true;
+
+  for (Eigen::Vector4d point : points) {
+
+    mrs_msgs::Reference reference;
+    reference.position.x = point[0];
+    reference.position.y = point[1];
+    reference.position.z = point[2];
+    reference.heading    = point[3];
+
+    path.points.push_back(reference);
+  }
+
+  // | ---------------- wait for ready to takeoff --------------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str());
+
+    if (mrsSystemReady()) {
+      ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str());
+      break;
+    }
+
+    sleep(0.01);
+  }
+
+  // | -------------------- call the service -------------------- |
+
+  {
+    auto [success, message] = setPathSrv(path);
+
+    if (!success) {
+      ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ------------------------- takeoff ------------------------ |
+
+  {
+    auto [success, message] = takeoff();
+
+    if (!success) {
+      ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ----------------------- goto start ----------------------- |
+
+  {
+    auto [success, message] = gotoTrajectoryStart();
+
+    if (!success) {
+      ROS_ERROR("[%s]: goto trajectory start failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ---------------- start trajectory tracking --------------- |
+
+  {
+    auto [success, message] = startTrajectoryTracking();
+
+    if (!success) {
+      ROS_ERROR("[%s]: start trajectory tracking failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | --------------- check if we pass each point -------------- |
+
+  {
+    auto [success, message] = checkPathFlythrough(points);
+
+    if (!success) {
+      ROS_ERROR("[%s]: path flythrough failed: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ---------- wait and check if we are still flying --------- |
+
+  sleep(5.0);
+
+  if (this->isFlyingNormally()) {
+    return true;
+  } else {
+    ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str());
+    return false;
+  }
+
+  ROS_ERROR("[%s]: reached the end of the test methd without assertion", 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();
+}