fxd land_home bug with moving takeoff reference frame

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);

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)

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)

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 <max_height>
+  height_check:
+
+    enabled: false

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

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

test/uav_manager/land_home_moving_takeoff_frame/land_home_moving_takeoff_frame.test

@@ -0,0 +1,38 @@
+<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)" />
+    <arg name="custom_config" default="$(dirname)/config/mrs_simulator.yaml" />
+  </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="automatic_start_config" default="$(dirname)/config/automatic_start.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>

test/uav_manager/land_home_moving_takeoff_frame/test.cpp

@@ -0,0 +1,163 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+
+#include <mrs_msgs/Float64Srv.h>
+
+class Tester : public mrs_uav_testing::TestGeneric {
+
+public:
+  bool test();
+
+  Tester();
+
+  bool asyncSetGroundZ();
+
+  mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv> sch_set_ground_z_;
+};
+
+Tester::Tester() {
+
+  sch_set_ground_z_ = mrs_lib::ServiceClientHandler<mrs_msgs::Float64Srv>(nh_, "/multirotor_simulator/uav1/set_ground_z");
+}
+
+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();
+  }
+
+  // | ------------- 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();
+}