added land_there test

launch/uav_manager.launch

@@ -84,6 +84,7 @@
       <remap from="~takeoff_in" to="~takeoff" />
       <remap from="~land_in" to="~land" />
       <remap from="~land_home_in" to="~land_home" />
+      <remap from="~land_there_in" to="~land_there" />
       <remap from="~midair_activation_in" to="~midair_activation" />
 
       <!-- Services out -->

src/uav_manager.cpp

@@ -1843,20 +1843,34 @@ bool UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req,
 
   ungripSrv();
 
-  mrs_msgs::ReferenceStamped reference_out;
+  auto odometry = sh_odometry_.getMsg();
+
+  // | ------ transform the reference to the current frame ------ |
+
+  mrs_msgs::ReferenceStamped reference_in;
+  reference_in.header    = req.header;
+  reference_in.reference = req.reference;
+
+  auto result = transformer_->transformSingle(reference_in, odometry->header.frame_id);
+
+  if (!result) {
+    std::stringstream ss;
+    ss << "can not transform the reference to the current control frame!";
+    res.message = ss.str();
+    res.success = false;
+    ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    return true;
+  }
 
   {
     std::scoped_lock lock(mutex_land_there_reference_);
 
-    land_there_reference_.header    = req.header;
-    land_there_reference_.reference = req.reference;
-
-    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_.header               = odometry->header;
+    land_there_reference_.reference            = reference_in.reference;
+    land_there_reference_.reference.position.z = odometry->pose.pose.position.z;
   }
 
-  bool service_success = emergencyReferenceSrv(reference_out);
+  bool service_success = emergencyReferenceSrv(land_there_reference_);
 
   if (service_success) {
 

test/uav_manager/CMakeLists.txt

@@ -4,6 +4,8 @@ add_subdirectory(landing)
 
 add_subdirectory(land_home)
 
+add_subdirectory(land_there)
+
 add_subdirectory(min_height_check)
 
 add_subdirectory(max_height_check)

test/uav_manager/land_there/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_there/config/mrs_simulator.yaml

@@ -0,0 +1,3 @@
+# turning off for the takeoff test
+individual_takeoff_platform:
+  enabled: false

test/uav_manager/land_there/config/world_config.yaml

@@ -0,0 +1,34 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 47.397743
+  origin_y: 8.545594
+
+safety_area:
+
+  enabled: false
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "world_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      -50, -50,
+      50,  -50,
+      50,  50,
+      -50, 50,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 15.0
+    min_z: 0.5

test/uav_manager/land_there/land_there.test

@@ -0,0 +1,37 @@
+<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="mrs_simulator_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="world_config" default="$(dirname)/config/world_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="60.0">
+    <param name="test" value="$(arg test_name)" />
+    <param name="uav_name" value="$(arg UAV_NAME)" />
+  </test>
+
+</launch>

test/uav_manager/land_there/test.cpp

@@ -0,0 +1,180 @@
+#include <gtest/gtest.h>
+
+#include <mrs_uav_testing/test_generic.h>
+#include <mrs_msgs/ReferenceStampedSrv.h>
+
+class Tester : public mrs_uav_testing::TestGeneric {
+
+public:
+  bool test();
+
+  Tester();
+
+  mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> sch_land_there_;
+
+  std::tuple<bool, std::string> landThere(const double x, const double y, const double z, const double hdg);
+};
+
+Tester::Tester() {
+
+  sch_land_there_ = mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>(nh_, "/" + _uav_name_ + "/uav_manager/land_there");
+}
+
+/* landThere() //{ */
+
+std::tuple<bool, std::string> Tester::landThere(const double x, const double y, const double z, const double hdg) {
+
+  if (!isFlyingNormally()) {
+    return {false, "not flying normally in the beginning"};
+  }
+
+  // | ----------------- call land there service ----------------- |
+
+  ROS_INFO("[%s]: calling for landing there", name_.c_str());
+
+  {
+    mrs_msgs::ReferenceStampedSrv srv;
+
+    srv.request.reference.position.x = x;
+    srv.request.reference.position.y = y;
+    srv.request.reference.position.z = z;
+    srv.request.reference.heading    = hdg;
+
+    {
+      bool service_call = sch_land_there_.call(srv);
+
+      if (!service_call || !srv.response.success) {
+        return {false, "land there service call failed"};
+      }
+    }
+  }
+
+  // | ---------------------- wait a second --------------------- |
+
+  sleep(1.0);
+
+  // | -------- wait till the right controller is active -------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return {false, "shut down from outside"};
+    }
+
+    if (sh_control_manager_diag_.getMsg()->active_tracker == "LandoffTracker" && sh_control_manager_diag_.getMsg()->active_controller == "MpcController") {
+      break;
+    }
+
+    sleep(0.01);
+  }
+
+  // | ------------- wait for the landing to finish ------------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return {false, "shut down from outside"};
+    }
+
+    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the landing to finish", name_.c_str());
+
+    if (!isOutputEnabled()) {
+
+      return {true, "landing finished"};
+    }
+
+    sleep(0.01);
+  }
+
+  return {false, "reached end of the method without assertion"};
+}
+
+//}
+
+bool Tester::test() {
+
+  // | ------------- wait for the system to be ready ------------ |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    if (this->mrsSystemReady()) {
+      break;
+    }
+  }
+
+  // | ------------------------ take off ------------------------ |
+
+  {
+    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;
+    }
+  }
+
+  sleep(1.0);
+
+  // | --------------------- goto somewhere --------------------- |
+
+  {
+    auto [success, message] = gotoAbs(10, 15, 5, 1.2);
+
+    if (!success) {
+      ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ----------------------- land there ----------------------- |
+
+  double des_x   = -10;
+  double des_y   = -15;
+  double des_z   = -10;
+  double des_hdg = 3.14;
+
+  {
+    auto [success, message] = landThere(des_x, des_y, des_z, des_hdg);
+
+    if (!success) {
+      ROS_ERROR("[%s]: land there failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+      return false;
+    }
+  }
+
+  // | ---------------- check the final position ---------------- |
+
+  if (this->isAtPosition(des_x, des_y, 0, des_hdg, 0.5)) {
+    return true;
+  } else {
+    ROS_ERROR("[%s]: land there did end in wrong place", 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();
+}