From f5e70e6668fb86bae9cef425fed9a79393e12356 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Thu, 16 Jan 2025 23:27:46 +0100 Subject: [PATCH] fixed RC mode with bumper, + BDD test --- src/control_manager/control_manager.cpp | 22 +- test/control_manager/CMakeLists.txt | 2 + .../bumper_with_rc_remote/CMakeLists.txt | 16 + .../bumper_with_rc_remote.test | 39 +++ .../config/custom_config.yaml | 27 ++ .../config/world_config.yaml | 34 ++ .../launch/hw_api.launch | 66 ++++ .../bumper_with_rc_remote/test.cpp | 301 ++++++++++++++++++ 8 files changed, 497 insertions(+), 10 deletions(-) create mode 100644 test/control_manager/bumper_with_rc_remote/CMakeLists.txt create mode 100644 test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test create mode 100644 test/control_manager/bumper_with_rc_remote/config/custom_config.yaml create mode 100644 test/control_manager/bumper_with_rc_remote/config/world_config.yaml create mode 100644 test/control_manager/bumper_with_rc_remote/launch/hw_api.launch create mode 100644 test/control_manager/bumper_with_rc_remote/test.cpp diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp index 6c2fb3b0..aabe131c 100644 --- a/src/control_manager/control_manager.cpp +++ b/src/control_manager/control_manager.cpp @@ -1844,14 +1844,14 @@ void ControlManager::initialize(void) { service_server_parachute_ = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this); service_server_set_min_z_ = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this); service_server_transform_reference_ = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this); - service_server_transform_reference_array_ = nh_.advertiseService("transform_reference_array_in", &ControlManager::callbackTransformReferenceArray, this); + service_server_transform_reference_array_ = nh_.advertiseService("transform_reference_array_in", &ControlManager::callbackTransformReferenceArray, this); service_server_transform_pose_ = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this); service_server_transform_vector3_ = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this); service_server_bumper_enabler_ = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this); service_server_get_min_z_ = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this); service_server_validate_reference_ = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this); service_server_validate_reference_2d_ = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this); - service_server_validate_reference_array_ = nh_.advertiseService("validate_reference_array_in", &ControlManager::callbackValidateReferenceArray, this); + service_server_validate_reference_array_ = nh_.advertiseService("validate_reference_array_in", &ControlManager::callbackValidateReferenceArray, this); service_server_start_trajectory_tracking_ = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this); service_server_stop_trajectory_tracking_ = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this); service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this); @@ -3168,7 +3168,7 @@ void ControlManager::timerJoystick(const ros::TimerEvent& event) { } } - if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) { + if (rc_goto_active_ && !bumper_repulsing_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) { // create the reference mrs_msgs::VelocityReferenceStampedSrv::Request request; @@ -3285,8 +3285,11 @@ void ControlManager::timerBumper(const ros::TimerEvent& event) { return; } - if (!isFlyingNormally() && !bumper_repulsing_) { - return; + if (!isFlyingNormally()) { + if (!(bumper_repulsing_ || rc_goto_active_)) { + ROS_WARN_THROTTLE(1.0, "[ControlManager]: bumpper can not function, not flying 'normally'"); + return; + } } if (!got_uav_state_) { @@ -5009,14 +5012,14 @@ bool ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenc } const auto tf = tf_opt.value(); - res.array.header.stamp = req.array.header.stamp; + res.array.header.stamp = req.array.header.stamp; res.array.header.frame_id = req.to_frame_id; res.array.array.reserve(req.array.array.size()); for (const auto& ref : req.array.array) { mrs_msgs::ReferenceStamped ref_stamped; - ref_stamped.header = req.array.header; + ref_stamped.header = req.array.header; ref_stamped.reference = ref; if (auto ret = transformer_->transform(ref_stamped, tf)) { @@ -5029,11 +5032,10 @@ bool ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenc res.success = false; return true; } - } - res.message = "transformation successful"; - res.success = true; + res.message = "transformation successful"; + res.success = true; return true; } diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt index 3dde48a6..504b8336 100644 --- a/test/control_manager/CMakeLists.txt +++ b/test/control_manager/CMakeLists.txt @@ -20,6 +20,8 @@ add_subdirectory(rc_remote_control) add_subdirectory(bumper) +add_subdirectory(bumper_with_rc_remote) + add_subdirectory(trajectory_topic) add_subdirectory(trajectory_start_stop_resume) diff --git a/test/control_manager/bumper_with_rc_remote/CMakeLists.txt b/test/control_manager/bumper_with_rc_remote/CMakeLists.txt new file mode 100644 index 00000000..bf8f9853 --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/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/bumper_with_rc_remote/bumper_with_rc_remote.test b/test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test new file mode 100644 index 00000000..ebfac5ca --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml b/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml new file mode 100644 index 00000000..4e77628e --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml @@ -0,0 +1,27 @@ +mrs_uav_managers: + + estimation_manager: + + state_estimators: [ + "gps_baro", + ] + + initial_state_estimator: "gps_baro" + + constraint_manager: + + default_constraints: + gps_garmin: "medium" + gps_baro: "medium" + + control_manager: + + rc_joystick: + + enabled: true + # activation channel + channel_number: 6 # indexed from 0 + + horizontal_speed: 4.00 # [m/s] + vertical_speed: 4.00 # [m/s] + heading_rate: 1.00 # [rad/s] diff --git a/test/control_manager/bumper_with_rc_remote/config/world_config.yaml b/test/control_manager/bumper_with_rc_remote/config/world_config.yaml new file mode 100644 index 00000000..00e01eef --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/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/bumper_with_rc_remote/launch/hw_api.launch b/test/control_manager/bumper_with_rc_remote/launch/hw_api.launch new file mode 100644 index 00000000..5e0f968a --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/launch/hw_api.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/control_manager/bumper_with_rc_remote/test.cpp b/test/control_manager/bumper_with_rc_remote/test.cpp new file mode 100644 index 00000000..7def418c --- /dev/null +++ b/test/control_manager/bumper_with_rc_remote/test.cpp @@ -0,0 +1,301 @@ +#include + +#include + +#include + +#include + +#include + +#include + +#define AIL 0 +#define THR 1 +#define ELE 2 +#define RUD 3 + +#define ACTIVATION 6 + +class Tester : public mrs_uav_testing::TestGeneric { + +public: + Tester(); + + bool test(); + +private: + mrs_lib::PublisherHandler ph_bumper_; + + ros::Timer timer_bumper_; + void timerBumper(const ros::TimerEvent& event); + + void setBumperFrontSector(const double distance); + + mrs_msgs::ObstacleSectors bumper_data_; + std::mutex mutex_bumper_data_; + + Eigen::Vector3d getBodyVelocity(); + + std::shared_ptr uh_; + + mrs_lib::PublisherHandler ph_rc_channels_; + + ros::Timer timer_rc_; + void timerRc(const ros::TimerEvent& event); + + mrs_msgs::HwApiRcChannels rc_; + std::mutex mutex_rc_; + + void activate(); + void deactivate(); + void moveForward(); +}; + +/* getBodyVelocity() //{ */ + +Eigen::Vector3d Tester::getBodyVelocity() { + + geometry_msgs::Vector3Stamped vel; + + auto uav_state = uh_->sh_uav_state_.getMsg(); + + vel.header = uav_state->header; + vel.vector = uav_state->velocity.linear; + + auto result = this->transformer_->transformSingle(vel, _uav_name_ + "/fcu_untilted"); + + if (result) { + return Eigen::Vector3d(result->vector.x, result->vector.y, result->vector.z); + } else { + ROS_ERROR("[%s]: could not transform body velocity", ros::this_node::getName().c_str()); + return Eigen::Vector3d(0, 0, 0); + } +} + +//} + +/* setBumeprFrontSector() //{ */ + +void Tester::setBumperFrontSector(const double distance) { + + std::scoped_lock lock(mutex_bumper_data_); + + bumper_data_.sectors[0] = distance; +} + +//} + +/* constructor Tester() //{ */ + +Tester::Tester() : mrs_uav_testing::TestGeneric() { + + ph_bumper_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/bumper/obstacle_sectors"); + + timer_bumper_ = nh_.createTimer(ros::Rate(20.0), &Tester::timerBumper, this); + + bumper_data_.n_horizontal_sectors = 8; + bumper_data_.sectors_vertical_fov = 20; + + for (unsigned int i = 0; i < bumper_data_.n_horizontal_sectors + 2; i++) { + bumper_data_.sectors.push_back(10.0); + } + + 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); +} + +//} + +/* timerBumper() //{ */ + +void Tester::timerBumper([[maybe_unused]] const ros::TimerEvent& event) { + + auto bumper_data = mrs_lib::get_mutexed(mutex_bumper_data_, bumper_data_); + + ph_bumper_.publish(bumper_data); +} + +//} + +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; + } +} + +bool Tester::test() { + + { + 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()); + return false; + } + } + + this->sleep(2.0); + + // | -------------- activate the RC joystick mode ------------- | + + activate(); + + this->sleep(0.5); + + { + auto ctrl_diag = uh_->sh_control_manager_diag_.getMsg(); + + if (!ctrl_diag->joystick_active) { + ROS_ERROR("[%s]: RC joystick not active when it should", ros::this_node::getName().c_str()); + return false; + } + } + + // | --------------------- flight forward --------------------- | + + moveForward(); + + this->sleep(5.0); + + { + auto body_vel = getBodyVelocity(); + + if (body_vel[0] < 1.0) { + ROS_ERROR("[%s]: forward velocity too small", ros::this_node::getName().c_str()); + return false; + } + } + + // | ------------------- activate the bumper ------------------ | + + ROS_INFO("[%s]: set the bumper front sector to 'close obstacle'", ros::this_node::getName().c_str()); + + setBumperFrontSector(0.5); + + this->sleep(5.0); + + { + auto ctrl_diag = uh_->sh_control_manager_diag_.getMsg(); + + if (!(!ctrl_diag->flying_normally && ctrl_diag->bumper_active)) { + ROS_ERROR("[%s]: missing the signs of the bumper being active", ros::this_node::getName().c_str()); + return false; + } + } + + { + auto body_vel = getBodyVelocity(); + + if (body_vel[0] > 0.0) { + ROS_ERROR("[%s]: forward velocity is positive, but it should not be", ros::this_node::getName().c_str()); + return false; + } + } + + ROS_INFO("[%s]: set the bumper front sector to 'none obstacle'", ros::this_node::getName().c_str()); + + setBumperFrontSector(10.0); + + ROS_INFO("[%s]: waiting for normal flight conditions", ros::this_node::getName().c_str()); + + while (true) { + + auto ctrl_diag = uh_->sh_control_manager_diag_.getMsg(); + + if (!ros::ok()) { + return false; + } + + auto body_vel = getBodyVelocity(); + + if (ctrl_diag->flying_normally && !ctrl_diag->bumper_active && body_vel[0] > 1.0) { + break; + } + } + + if (uh_->isFlyingNormally()) { + return true; + } else { + ROS_ERROR("[%s]: not flying normally", 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(); +}