diff --git a/include/mrs_uav_testing/test_generic.h b/include/mrs_uav_testing/test_generic.h index bcead62..f90da25 100644 --- a/include/mrs_uav_testing/test_generic.h +++ b/include/mrs_uav_testing/test_generic.h @@ -36,6 +36,9 @@ #include #include + +#include + //} namespace mrs_uav_testing @@ -51,9 +54,9 @@ using namespace std; class UAVHandler { public: - UAVHandler(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim); + UAVHandler(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim, bool use_hw_api = true); - void initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim); + void initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim, bool use_hw_api = true); tuple spawn(string gazebo_spawner_params); @@ -87,6 +90,9 @@ class UAVHandler { std::optional getHeightAgl(void); std::optional getCurrentConstraints(void); + + tuple moveTo(double x, double y, double z, double hdg); + tuple setPathSrv(const mrs_msgs::Path &path_in); tuple setPathTopic(const mrs_msgs::Path &path_in); tuple switchEstimator(const std::string &estimator); @@ -130,6 +136,8 @@ class UAVHandler { mrs_lib::SubscribeHandler sh_hw_api_status_; + mrs_lib::PublisherHandler ph_gazebo_model_state_; + protected: bool initialized_ = false; bool spawned_ = false; @@ -142,6 +150,7 @@ class UAVHandler { mrs_lib::SubscribeHandlerOptions shopts_; ros::NodeHandle nh_; string name_; + bool use_hw_api_ = true; }; //} @@ -159,8 +168,9 @@ class TestGeneric { std::shared_ptr pl_; - UAVHandler getUAVHandler(string uav_name); - bool isGazeboSimulation(void); + std::tuple>, string> getUAVHandler(const string &uav_name, const bool use_hw_api = true); + + bool isGazeboSimulation(void); void sleep(const double &duration); @@ -178,6 +188,9 @@ class TestGeneric { string _test_name_; string name_; + + bool initialized_ = false; + bool mrsSystemReady(void); private: diff --git a/src/test_generic.cpp b/src/test_generic.cpp index e0b1bf8..41c25ba 100644 --- a/src/test_generic.cpp +++ b/src/test_generic.cpp @@ -3,18 +3,12 @@ namespace mrs_uav_testing { -/* UAVHandler(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim) //{ */ +UAVHandler::UAVHandler(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim, bool use_hw_api) { -UAVHandler::UAVHandler(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim) { - - initialize(uav_name, shopts, using_gazebo_sim); + initialize(uav_name, shopts, using_gazebo_sim, use_hw_api); } -//} - -/* initialize() //{ */ - -void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim) { +void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptions shopts, bool using_gazebo_sim, bool use_hw_api) { _uav_name_ = uav_name; shopts_ = shopts; @@ -23,6 +17,8 @@ void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptio is_gazebo_simulation_ = using_gazebo_sim; + use_hw_api_ = use_hw_api; + sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts_, "/" + _uav_name_ + "/control_manager/diagnostics"); sh_current_constraints_ = mrs_lib::SubscribeHandler(shopts_, "/" + _uav_name_ + "/control_manager/current_constraints"); sh_uav_manager_diag_ = mrs_lib::SubscribeHandler(shopts_, "/" + _uav_name_ + "/uav_manager/diagnostics"); @@ -62,6 +58,7 @@ void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptio ph_path_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/trajectory_generation/path"); + ph_gazebo_model_state_ = mrs_lib::PublisherHandler(nh_, "/gazebo/set_model_state"); // | --------------------- service clients -------------------- | // TODO: is it an issue that each UAVHandler has its own spawn service client? @@ -123,6 +120,7 @@ void TestGeneric::initialize(void) { // | --------------------- finish the init -------------------- | + initialized_ = true; ROS_INFO("[%s]: initialized", name_.c_str()); } @@ -188,27 +186,29 @@ tuple UAVHandler::spawn(string gazebo_spawner_params) { sleep(0.01); } - // | ------------- wait for the HW API to connect ------------- | + if (use_hw_api_) { + // | ------------- wait for the HW API to connect ------------- | - while (true) { + while (true) { - if (!ros::ok()) { - return {false, "shut down from outside"}; - } + if (!ros::ok()) { + return {false, "shut down from outside"}; + } - ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the hw API", name_.c_str()); + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the hw API", name_.c_str()); - if (sh_hw_api_status_.hasMsg()) { - if (sh_hw_api_status_.getMsg()->connected) { - break; + if (sh_hw_api_status_.hasMsg()) { + if (sh_hw_api_status_.getMsg()->connected) { + break; + } } + + sleep(0.01); } - sleep(0.01); + // | -------------- wait for PX4 to finish bootup ------------- | } - // | -------------- wait for PX4 to finish bootup ------------- | - sleep(20.0); spawned_ = true; @@ -259,13 +259,15 @@ bool TestGeneric::isGazeboSimulation(void) { return false; } -//} - /* getUAVHandler() //{ */ -UAVHandler TestGeneric::getUAVHandler(string uav_name) { +std::tuple>, string> TestGeneric::getUAVHandler(const string &uav_name, const bool use_hw_api) { - return UAVHandler(uav_name, shopts_, isGazeboSimulation()); + if (!initialized_) { + return {std::nullopt, string("Can not spawn " + uav_name + " - testing is not initialized yet!")}; + } else { + return {std::make_shared(uav_name, shopts_, isGazeboSimulation(), use_hw_api), "Success!"}; + } } //} @@ -1029,6 +1031,33 @@ std::optional UAVHandler::getCurrentConstraints(v //} +/* moveTo() Implements direct transporting of a UAVs in the simulation//{ */ +tuple UAVHandler::moveTo(double x, double y, double z, double hdg) { + + if (is_gazebo_simulation_) { + gazebo_msgs::ModelState msg; + msg.model_name = _uav_name_; + + msg.pose.position.x = x; + msg.pose.position.y = y; + msg.pose.position.z = z; + + double qw = cos(hdg / 2.0); + double qz = sin(hdg / 2.0); + msg.pose.orientation.x = 0; + msg.pose.orientation.y = 0; + msg.pose.orientation.z = qz; + msg.pose.orientation.w = qw; + + ph_gazebo_model_state_.publish(msg); + return {true, "Success!"}; + } else { + ROS_ERROR_ONCE("[%s]: Direct moving of UAVs outside of Gazebo simulation is not implemented!", ros::this_node::getName().c_str()); + return {false, "Direct moving of UAVs outside of Gazebo simulation is not implemented!"}; + } +} +//} + /* getTrackerCmd() //{ */ std::optional UAVHandler::getTrackerCmd(void) { diff --git a/test/goto/test.cpp b/test/goto/test.cpp index 396464c..7e36570 100644 --- a/test/goto/test.cpp +++ b/test/goto/test.cpp @@ -10,10 +10,21 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto uh = getUAVHandler(_uav_name_); + std::shared_ptr uh; { - auto [success, message] = uh.activateMidAir(); + 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()); @@ -22,7 +33,7 @@ bool Tester::test() { } { - auto [success, message] = uh.gotoAbs(0, 0, 2.0, 0); + auto [success, message] = uh->gotoAbs(0, 0, 2.0, 0); if (!success) { ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -32,7 +43,7 @@ bool Tester::test() { sleep(5.0); - if (uh.isFlyingNormally()) { + if (uh->isFlyingNormally()) { return true; } else { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); diff --git a/test/goto_relative/test.cpp b/test/goto_relative/test.cpp index e11784d..0765079 100644 --- a/test/goto_relative/test.cpp +++ b/test/goto_relative/test.cpp @@ -10,10 +10,21 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto uh = getUAVHandler(_uav_name_); + std::shared_ptr uh; { - auto [success, message] = uh.activateMidAir(); + 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()); @@ -22,7 +33,7 @@ bool Tester::test() { } { - auto [success, message] = uh.gotoRel(1, 2, 3, 1); + auto [success, message] = uh->gotoRel(1, 2, 3, 1); if (!success) { ROS_ERROR("[%s]: goto relative failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -32,7 +43,7 @@ bool Tester::test() { sleep(5.0); - if (uh.isFlyingNormally()) { + if (uh->isFlyingNormally()) { return true; } else { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); diff --git a/test/takeoff/test.cpp b/test/takeoff/test.cpp index 8903bad..ac0abc2 100644 --- a/test/takeoff/test.cpp +++ b/test/takeoff/test.cpp @@ -10,9 +10,20 @@ class Tester : public mrs_uav_testing::TestGeneric { bool Tester::test() { - auto uh = getUAVHandler(_uav_name_); + std::shared_ptr uh; - auto [success, message] = uh.takeoff(); + { + 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->takeoff(); if (!success) { ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); @@ -21,7 +32,7 @@ bool Tester::test() { sleep(5.0); - if (uh.isFlyingNormally()) { + if (uh->isFlyingNormally()) { return true; } else { ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str());