Skip to content

Commit

Permalink
refactored test interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@klaxalk
klaxalk committed Feb 11, 2024
1 parent 5664b12 commit 9775c50
Show file tree
Hide file tree
Showing 5 changed files with 115 additions and 40 deletions.
21 changes: 17 additions & 4 deletions include/mrs_uav_testing/test_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>


#include <gazebo_msgs/ModelState.h>

//}

namespace mrs_uav_testing
Expand All @@ -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<bool, string> spawn(string gazebo_spawner_params);

Expand Down Expand Up @@ -87,6 +90,9 @@ class UAVHandler {
std::optional<double> getHeightAgl(void);
std::optional<mrs_msgs::DynamicsConstraints> getCurrentConstraints(void);


tuple<bool, string> moveTo(double x, double y, double z, double hdg);

tuple<bool, string> setPathSrv(const mrs_msgs::Path &path_in);
tuple<bool, string> setPathTopic(const mrs_msgs::Path &path_in);
tuple<bool, string> switchEstimator(const std::string &estimator);
Expand Down Expand Up @@ -130,6 +136,8 @@ class UAVHandler {

mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;

mrs_lib::PublisherHandler<gazebo_msgs::ModelState> ph_gazebo_model_state_;

protected:
bool initialized_ = false;
bool spawned_ = false;
Expand All @@ -142,6 +150,7 @@ class UAVHandler {
mrs_lib::SubscribeHandlerOptions shopts_;
ros::NodeHandle nh_;
string name_;
bool use_hw_api_ = true;
};

//}
Expand All @@ -159,8 +168,9 @@ class TestGeneric {

std::shared_ptr<mrs_lib::ParamLoader> pl_;

UAVHandler getUAVHandler(string uav_name);
bool isGazeboSimulation(void);
std::tuple<std::optional<std::shared_ptr<UAVHandler>>, string> getUAVHandler(const string &uav_name, const bool use_hw_api = true);

bool isGazeboSimulation(void);

void sleep(const double &duration);

Expand All @@ -178,6 +188,9 @@ class TestGeneric {
string _test_name_;
string name_;


bool initialized_ = false;

bool mrsSystemReady(void);

private:
Expand Down
79 changes: 54 additions & 25 deletions src/test_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<mrs_msgs::ControlManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/control_manager/diagnostics");
sh_current_constraints_ = mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints>(shopts_, "/" + _uav_name_ + "/control_manager/current_constraints");
sh_uav_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/uav_manager/diagnostics");
Expand Down Expand Up @@ -62,6 +58,7 @@ void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptio

ph_path_ = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "/" + _uav_name_ + "/trajectory_generation/path");

ph_gazebo_model_state_ = mrs_lib::PublisherHandler<gazebo_msgs::ModelState>(nh_, "/gazebo/set_model_state");
// | --------------------- service clients -------------------- |

// TODO: is it an issue that each UAVHandler has its own spawn service client?
Expand Down Expand Up @@ -123,6 +120,7 @@ void TestGeneric::initialize(void) {

// | --------------------- finish the init -------------------- |

initialized_ = true;
ROS_INFO("[%s]: initialized", name_.c_str());
}

Expand Down Expand Up @@ -188,27 +186,29 @@ tuple<bool, string> 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;
Expand Down Expand Up @@ -259,13 +259,15 @@ bool TestGeneric::isGazeboSimulation(void) {
return false;
}

//}

/* getUAVHandler() //{ */

UAVHandler TestGeneric::getUAVHandler(string uav_name) {
std::tuple<std::optional<std::shared_ptr<UAVHandler>>, 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<UAVHandler>(uav_name, shopts_, isGazeboSimulation(), use_hw_api), "Success!"};
}
}

//}
Expand Down Expand Up @@ -1029,6 +1031,33 @@ std::optional<mrs_msgs::DynamicsConstraints> UAVHandler::getCurrentConstraints(v

//}

/* moveTo() Implements direct transporting of a UAVs in the simulation//{ */
tuple<bool, string> 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<mrs_msgs::TrackerCommand> UAVHandler::getTrackerCmd(void) {
Expand Down
19 changes: 15 additions & 4 deletions test/goto/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,21 @@ class Tester : public mrs_uav_testing::TestGeneric {

bool Tester::test() {

auto uh = getUAVHandler(_uav_name_);
std::shared_ptr<mrs_uav_testing::UAVHandler> 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());
Expand All @@ -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());
Expand All @@ -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());
Expand Down
19 changes: 15 additions & 4 deletions test/goto_relative/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,21 @@ class Tester : public mrs_uav_testing::TestGeneric {

bool Tester::test() {

auto uh = getUAVHandler(_uav_name_);
std::shared_ptr<mrs_uav_testing::UAVHandler> 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());
Expand All @@ -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());
Expand All @@ -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());
Expand Down
17 changes: 14 additions & 3 deletions test/takeoff/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,20 @@ class Tester : public mrs_uav_testing::TestGeneric {

bool Tester::test() {

auto uh = getUAVHandler(_uav_name_);
std::shared_ptr<mrs_uav_testing::UAVHandler> 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());
Expand All @@ -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());
Expand Down

0 comments on commit 9775c50

Please sign in to comment.