From 35da01ac39ad31ad09cabd014d686f952d3b6da2 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sat, 6 Jan 2024 10:08:26 +0100 Subject: [PATCH] added rc remote test --- test/control_manager/CMakeLists.txt | 2 + .../escalating_failsafe_rc/test.cpp | 8 +- .../rc_remote_control/CMakeLists.txt | 16 + .../config/custom_config.yaml | 7 + .../config/world_config.yaml | 34 ++ .../rc_remote_control/launch/hw_api.launch | 66 +++ .../rc_remote_control/rc_remote_control.test | 37 ++ .../rc_remote_control/test.cpp | 215 +++++++ test/include/remote_control_test.h | 531 ++++++++++++++++++ 9 files changed, 912 insertions(+), 4 deletions(-) create mode 100644 test/control_manager/rc_remote_control/CMakeLists.txt create mode 100644 test/control_manager/rc_remote_control/config/custom_config.yaml create mode 100644 test/control_manager/rc_remote_control/config/world_config.yaml create mode 100644 test/control_manager/rc_remote_control/launch/hw_api.launch create mode 100644 test/control_manager/rc_remote_control/rc_remote_control.test create mode 100644 test/control_manager/rc_remote_control/test.cpp create mode 100644 test/include/remote_control_test.h diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt index 725333ea..b612632d 100644 --- a/test/control_manager/CMakeLists.txt +++ b/test/control_manager/CMakeLists.txt @@ -11,3 +11,5 @@ add_subdirectory(eland_service) add_subdirectory(escalating_failsafe_service) add_subdirectory(escalating_failsafe_rc) + +add_subdirectory(rc_remote_control) diff --git a/test/control_manager/escalating_failsafe_rc/test.cpp b/test/control_manager/escalating_failsafe_rc/test.cpp index 512c4ff6..7510c3dd 100644 --- a/test/control_manager/escalating_failsafe_rc/test.cpp +++ b/test/control_manager/escalating_failsafe_rc/test.cpp @@ -24,10 +24,10 @@ Tester::Tester() { ph_rc_channels_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/rc_channels"); - rc_.channels.push_back(0.0); - rc_.channels.push_back(0.0); - rc_.channels.push_back(0.0); - rc_.channels.push_back(0.0); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); rc_.channels.push_back(0.0); rc_.channels.push_back(0.0); rc_.channels.push_back(0.0); diff --git a/test/control_manager/rc_remote_control/CMakeLists.txt b/test/control_manager/rc_remote_control/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/rc_remote_control/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) diff --git a/test/control_manager/rc_remote_control/config/custom_config.yaml b/test/control_manager/rc_remote_control/config/custom_config.yaml new file mode 100644 index 00000000..28b96e68 --- /dev/null +++ b/test/control_manager/rc_remote_control/config/custom_config.yaml @@ -0,0 +1,7 @@ +mrs_uav_managers: + + constraint_manager: + + default_constraints: + gps_garmin: "medium" + gps_baro: "medium" diff --git a/test/control_manager/rc_remote_control/config/world_config.yaml b/test/control_manager/rc_remote_control/config/world_config.yaml new file mode 100644 index 00000000..00e01eef --- /dev/null +++ b/test/control_manager/rc_remote_control/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 diff --git a/test/control_manager/rc_remote_control/launch/hw_api.launch b/test/control_manager/rc_remote_control/launch/hw_api.launch new file mode 100644 index 00000000..5e0f968a --- /dev/null +++ b/test/control_manager/rc_remote_control/launch/hw_api.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/rc_remote_control/rc_remote_control.test b/test/control_manager/rc_remote_control/rc_remote_control.test new file mode 100644 index 00000000..54865e18 --- /dev/null +++ b/test/control_manager/rc_remote_control/rc_remote_control.test @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/rc_remote_control/test.cpp b/test/control_manager/rc_remote_control/test.cpp new file mode 100644 index 00000000..de8cf3ea --- /dev/null +++ b/test/control_manager/rc_remote_control/test.cpp @@ -0,0 +1,215 @@ +#include + +#include + +#include + +#define AIL 0 +#define THR 1 +#define ELE 2 +#define RUD 3 + +#define ACTIVATION 6 + +class Tester : public RemoteControlTest { + +public: + Tester(); + + void activate(); + void deactivate(); + + void moveForward(); + void moveBackward(); + void moveLeft(); + void moveRight(); + void moveUp(); + void moveDown(); + void rotateLeft(); + void rotateRight(); + + void stop(); + + mrs_lib::PublisherHandler ph_rc_channels_; + + ros::Timer timer_rc_; + void timerRc(const ros::TimerEvent& event); + + mrs_msgs::HwApiRcChannels rc_; + std::mutex mutex_rc_; +}; + +Tester::Tester() { + + ph_rc_channels_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/rc_channels"); + + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.5); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + rc_.channels.push_back(0.0); + + timer_rc_ = nh_.createTimer(ros::Rate(100.0), &Tester::timerRc, this, false, true); +} + +void Tester::timerRc([[maybe_unused]] const ros::TimerEvent& event) { + + { + std::scoped_lock lock(mutex_rc_); + + ph_rc_channels_.publish(rc_); + } +} + +void Tester::activate() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ACTIVATION] = 1.0; + } +} + +void Tester::deactivate() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ACTIVATION] = 0.0; + } +} + +void Tester::moveForward() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 1.0; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.5; + } +} + +void Tester::moveBackward() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.0; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.5; + } +} + +void Tester::moveLeft() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.0; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.5; + } +} + +void Tester::moveRight() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 1.0; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.5; + } +} + +void Tester::moveUp() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 1.0; + } +} + +void Tester::moveDown() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.0; + } +} + +void Tester::rotateLeft() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.0; + rc_.channels[THR] = 0.5; + } +} + +void Tester::rotateRight() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 1.0; + rc_.channels[THR] = 0.5; + } +} + +void Tester::stop() { + + { + std::scoped_lock lock(mutex_rc_); + + rc_.channels[ELE] = 0.5; + rc_.channels[AIL] = 0.5; + rc_.channels[RUD] = 0.5; + rc_.channels[THR] = 0.5; + } +} + +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(); +} diff --git a/test/include/remote_control_test.h b/test/include/remote_control_test.h new file mode 100644 index 00000000..3cc68ee6 --- /dev/null +++ b/test/include/remote_control_test.h @@ -0,0 +1,531 @@ +#include + +class RemoteControlTest : public mrs_uav_testing::TestGeneric { + +public: + bool test(); + + RemoteControlTest() : mrs_uav_testing::TestGeneric(){}; + + /* interface to be implemented by the particular test //{ */ + + virtual void activate() = 0; + virtual void deactivate() = 0; + + virtual void moveForward() = 0; + virtual void moveBackward() = 0; + virtual void moveLeft() = 0; + virtual void moveRight() = 0; + virtual void moveUp() = 0; + virtual void moveDown() = 0; + virtual void rotateLeft() = 0; + virtual void rotateRight() = 0; + + virtual void stop() = 0; + + //} + + bool testMoveForward(); + bool testMoveBackward(); + bool testMoveLeft(); + bool testMoveRight(); + bool testMoveUp(); + bool testMoveDown(); + bool testRotateLeft(); + bool testRotateRight(); + bool testStop(); + + Eigen::Vector3d getFcuUntiltedVelocity(); + + bool setGotoReference(); +}; + +/* getFcuUntiltedVelocity() //{ */ + +Eigen::Vector3d RemoteControlTest::getFcuUntiltedVelocity() { + + auto uav_state = sh_uav_state_.getMsg(); + + geometry_msgs::Vector3Stamped vel_world; + vel_world.header = uav_state->header; + vel_world.vector = uav_state->velocity.linear; + + auto vel_fcu_untilted = transformer_->transformSingle(vel_world, "fcu_untilted"); + + if (vel_fcu_untilted) { + return Eigen::Vector3d(vel_fcu_untilted->vector.x, vel_fcu_untilted->vector.y, vel_fcu_untilted->vector.z); + } else { + return Eigen::Vector3d::Zero(); + } +} + +//} + +/* testMoveForward() //{ */ + +bool RemoteControlTest::testMoveForward() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveForward(); + + auto velocity = getFcuUntiltedVelocity(); + + if (velocity[0] > 0.5 && abs(velocity[1]) < 0.1 && abs(velocity[2]) < 0.1) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testMoveBackward() //{ */ + +bool RemoteControlTest::testMoveBackward() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveBackward(); + + auto velocity = getFcuUntiltedVelocity(); + + if (velocity[0] < -0.5 && abs(velocity[1]) < 0.1 && abs(velocity[2]) < 0.1) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testMoveLeft() //{ */ + +bool RemoteControlTest::testMoveLeft() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveLeft(); + + auto velocity = getFcuUntiltedVelocity(); + + if (abs(velocity[0]) < 0.1 && velocity[1] > 0.5 && abs(velocity[2]) < 0.1) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testMoveRight() //{ */ + +bool RemoteControlTest::testMoveRight() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveRight(); + + auto velocity = getFcuUntiltedVelocity(); + + if (abs(velocity[0]) < 0.1 && velocity[1] < -0.5 && abs(velocity[2]) < 0.1) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testMoveUp() //{ */ + +bool RemoteControlTest::testMoveUp() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveUp(); + + auto velocity = getFcuUntiltedVelocity(); + + if (abs(velocity[0]) < 0.1 && abs(velocity[1]) < 0.1 && velocity[2] > 0.5) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testMoveDown() //{ */ + +bool RemoteControlTest::testMoveDown() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + moveDown(); + + auto velocity = getFcuUntiltedVelocity(); + + if (abs(velocity[0]) < 0.1 && abs(velocity[1]) < 0.1 && velocity[2] < -0.5) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testRotateLeft() //{ */ + +bool RemoteControlTest::testRotateLeft() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + rotateLeft(); + + auto uav_state = sh_uav_state_.getMsg(); + + if (uav_state->velocity.angular.z > 0.2) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testRotateRight() //{ */ + +bool RemoteControlTest::testRotateRight() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + rotateRight(); + + auto uav_state = sh_uav_state_.getMsg(); + + if (uav_state->velocity.angular.z < -0.2) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 5.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* testStop() //{ */ + +bool RemoteControlTest::testStop() { + + ros::Time started = ros::Time::now(); + + while (true) { + + if (!ros::ok()) { + return false; + } + + stop(); + + auto velocity = getFcuUntiltedVelocity(); + auto uav_state = sh_uav_state_.getMsg(); + + if (abs(velocity[0]) < 0.1 && abs(velocity[1]) < 0.1 && abs(velocity[2]) < 0.1 && abs(uav_state->velocity.angular.z) < 0.1) { + return true; + } + + if ((ros::Time::now() - started).toSec() > 10.0) { + return false; + } + + sleep(0.1); + } +} + +//} + +/* setGotoReference() //{ */ + +bool RemoteControlTest::setGotoReference() { + + mrs_msgs::Vec4 srv; + + srv.request.goal[0] = 1000; + srv.request.goal[1] = 0; + srv.request.goal[2] = 5; + srv.request.goal[3] = 0; + + { + bool service_call = sch_goto_relative_.call(srv); + + if (!service_call || !srv.response.success) { + ROS_ERROR("[%s]: failed to call the service", ros::this_node::getName().c_str()); + return false; + } + } + + sleep(5.0); + + // check if we gained velocity + + auto uav_state = sh_uav_state_.getMsg(); + + if (uav_state->velocity.linear.x > 1.0) { + return true; + } else { + ROS_ERROR("[%s]: velocity not reached, %.2f, %.2f, %.2f", ros::this_node::getName().c_str(), uav_state->velocity.linear.x, uav_state->velocity.linear.y, + uav_state->velocity.linear.z); + return false; + } +} + +//} + +bool RemoteControlTest::test() { + + { + auto [success, message] = activateMidAir(); + + if (!success) { + ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------- activate the remote controller ------------- | + + activate(); + + // | -------- wait for indication from the diagnostics -------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (sh_control_manager_diag_.getMsg()->rc_mode) { + break; + } + + sleep(0.01); + } + + // | -------------------- test the motions -------------------- | + + { + bool success = testRotateLeft(); + + if (!success) { + ROS_ERROR("[%s]: left rotation motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testRotateRight(); + + if (!success) { + ROS_ERROR("[%s]: right rotation motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveForward(); + + if (!success) { + ROS_ERROR("[%s]: forward motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveBackward(); + + if (!success) { + ROS_ERROR("[%s]: backward motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveLeft(); + + if (!success) { + ROS_ERROR("[%s]: left motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveRight(); + + if (!success) { + ROS_ERROR("[%s]: right motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveUp(); + + if (!success) { + ROS_ERROR("[%s]: up motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testMoveDown(); + + if (!success) { + ROS_ERROR("[%s]: down motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + deactivate(); + + // | -------- wait for indication from the diagnostics -------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + if (!sh_control_manager_diag_.getMsg()->rc_mode) { + break; + } + + sleep(0.01); + } + + sleep(2.0); + + // | ------ test if we can override a pre-existing motion ----- | + + { + bool success = setGotoReference(); + + if (!success) { + ROS_ERROR("[%s]: failed to set goto reference", ros::this_node::getName().c_str()); + return false; + } + } + + activate(); + + { + bool success = testMoveUp(); + + if (!success) { + ROS_ERROR("[%s]: up motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + { + bool success = testStop(); + + if (!success) { + ROS_ERROR("[%s]: stop motion test failed", ros::this_node::getName().c_str()); + return false; + } + } + + deactivate(); + + // | ------------- activate the remote controller ------------- | + + return true; +}