diff --git a/rj_msgs/CMakeLists.txt b/rj_msgs/CMakeLists.txt index 49d8df8f77..6420912f7c 100644 --- a/rj_msgs/CMakeLists.txt +++ b/rj_msgs/CMakeLists.txt @@ -52,6 +52,7 @@ rosidl_generate_interfaces( msg/TeamInfo.msg msg/Trajectory.msg msg/WorldState.msg + msg/OverridePosition.msg # Communication msg/AgentRequest.msg diff --git a/rj_msgs/msg/OverridePosition.msg b/rj_msgs/msg/OverridePosition.msg new file mode 100644 index 0000000000..9ba10cf35a --- /dev/null +++ b/rj_msgs/msg/OverridePosition.msg @@ -0,0 +1,6 @@ +# Contains two unsigned ints - The robot ID to override and the position +# to set for that ID, based on the enum OverridingPositions, +# which is contained in soccer/src/soccer/strategy/agent/position/overriding_positions.hpp + +uint32 robot_id +uint32 overriding_position \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/overriding_positions.hpp b/soccer/src/soccer/strategy/agent/position/overriding_positions.hpp new file mode 100644 index 0000000000..e0effea2a2 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/overriding_positions.hpp @@ -0,0 +1,28 @@ +#pragma once +#include +#include +#include + +namespace Strategy { +/* + OverridingPositions refers to the positions that can be manually set in the UI. + Normally, all robots are set to Auto. If you want to add a new position, add a new value + to this enum before LENGTH, add a string to the overriding_position_labels vector in + main_window.hpp, and add a case to the check_for_position_override method in + RobotFactoryPosition. +*/ +enum OverridingPositions { + AUTO, + OFFENSE, + DEFENSE, + FREE_KICKER, + PENALTY_PLAYER, + PENALTY_NON_KICKER, + SOLO_OFFENSE, + SMART_IDLE, + ZONER, + IDLE, + LENGTH, // Do not remove +}; + +} // namespace Strategy diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 7634ff9622..1eab1fbbfd 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -15,6 +15,14 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id, "RobotFact } else { current_position_ = std::make_unique(robot_id_); } + + std::string node_name{"robot_factory_position_"}; + _node = std::make_shared(node_name.append(std::to_string(robot_id_))); + override_play_sub_ = _node->create_subscription( + "override_position_for_robot", 1, + [this](const rj_msgs::msg::OverridePosition::SharedPtr msg) { test_play_callback(msg); }); + _executor.add_node(_node); + _executor_thread = std::thread([this]() { _executor.spin(); }); } std::optional RobotFactoryPosition::derived_get_task([ @@ -119,6 +127,9 @@ void RobotFactoryPosition::handle_ready() { } void RobotFactoryPosition::update_position() { + bool manual_position_set = check_for_position_override(); + if (manual_position_set) return; + switch (current_play_state_.state()) { case PlayState::State::Playing: { // We just became regular playing. @@ -338,4 +349,61 @@ std::string RobotFactoryPosition::get_current_state() { return current_position_->get_current_state(); } +void RobotFactoryPosition::test_play_callback( + const rj_msgs::msg::OverridePosition::SharedPtr message) { + if (message->robot_id == this->robot_id_ && + message->overriding_position < Strategy::OverridingPositions::LENGTH) { + override_play_position_ = + static_cast(message->overriding_position); + } +} + +/** + * Checks override_play_position_, which automatically updates when an override is set. + * If it is anything but auto, set the current position to that position and return true. + */ +bool RobotFactoryPosition::check_for_position_override() { + switch (override_play_position_) { + case Strategy::OverridingPositions::OFFENSE: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::DEFENSE: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::FREE_KICKER: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::PENALTY_PLAYER: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::PENALTY_NON_KICKER: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::SMART_IDLE: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::SOLO_OFFENSE: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::ZONER: { + set_current_position(); + return true; + } + case Strategy::OverridingPositions::IDLE: { + set_current_position(); + return true; + } + default: { + return false; + } + } +} + } // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index 7dad7c6db7..54659880a5 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -10,17 +10,20 @@ #include #include "game_state.hpp" +#include "overriding_positions.hpp" #include "planning/instant.hpp" #include "position.hpp" #include "rj_common/field_dimensions.hpp" #include "rj_common/time.hpp" #include "rj_constants/constants.hpp" #include "rj_geometry/geometry_conversions.hpp" +#include "rj_msgs/msg/override_position.hpp" #include "strategy/agent/position/defense.hpp" #include "strategy/agent/position/free_kicker.hpp" #include "strategy/agent/position/goal_kicker.hpp" #include "strategy/agent/position/goalie.hpp" #include "strategy/agent/position/offense.hpp" +#include "strategy/agent/position/overriding_positions.hpp" #include "strategy/agent/position/penalty_non_kicker.hpp" #include "strategy/agent/position/penalty_player.hpp" #include "strategy/agent/position/pivot_test.hpp" @@ -109,6 +112,14 @@ class RobotFactoryPosition : public Position { private: std::unique_ptr current_position_; + // subscription for test mode - Allows position overriding + rclcpp::Subscription::SharedPtr override_play_sub_; + rclcpp::executors::SingleThreadedExecutor _executor; + std::thread _executor_thread; + void test_play_callback(const rj_msgs::msg::OverridePosition::SharedPtr message); + Strategy::OverridingPositions override_play_position_{Strategy::OverridingPositions::AUTO}; + rclcpp::Node::SharedPtr _node; + std::optional derived_get_task(RobotIntent intent) override; bool am_closest_kicker(); @@ -153,6 +164,8 @@ class RobotFactoryPosition : public Position { current_position_ = std::make_unique(*current_position_); } } + + bool check_for_position_override(); }; } // namespace strategy diff --git a/soccer/src/soccer/ui/main_window.cpp b/soccer/src/soccer/ui/main_window.cpp index fbc1e3cbd5..b8e8ca6ed1 100644 --- a/soccer/src/soccer/ui/main_window.cpp +++ b/soccer/src/soccer/ui/main_window.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -10,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -17,6 +19,7 @@ #include #include +#include #include #include #include @@ -49,6 +52,7 @@ void calcMinimumWidth(QWidget* widget, const QString& text) { widget->setMinimumWidth(rect.width()); } +// TODO: Set up the UI objects and bind them to methods MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* parent) : QMainWindow(parent), _updateCount(0), @@ -185,8 +189,19 @@ MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* par _set_game_settings = _node->create_client( config_server::topics::kGameSettingsSrv); + override_play_pub_ = + _node->create_publisher("override_position_for_robot", 1); + _executor.add_node(_node); _executor_thread = std::thread([this]() { _executor.spin(); }); + + // Connect up all the test position buttons and dropdowns to their listeners + for (int i = 0; i < kNumShells; ++i) { + robot_pos_selectors[i] = + findChild(QString::fromStdString("robotPosition_" + std::to_string(i))); + position_reset_buttons[i] = + findChild(QString::fromStdString("positionReset_" + std::to_string(i))); + } } void MainWindow::initialize() { @@ -197,6 +212,8 @@ void MainWindow::initialize() { _ui.actionTeamYellow->trigger(); } + populate_override_position_dropdowns(); + logFileChanged(); // Initialize to ui defaults @@ -1033,6 +1050,14 @@ void MainWindow::on_actionUse_Multiple_Joysticks_toggled(bool value) { void MainWindow::on_goalieID_currentIndexChanged(int value) { update_cache(_game_settings.request_goalie_id, value - 1, &_game_settings_valid); + QString goalieNum = _ui.goalieID->currentText(); + bool goalieNumIsInt{false}; + int goalieInt = goalieNum.toInt(&goalieNumIsInt); + current_goalie_num_ = goalieInt; + if (goalieNumIsInt) + setGoalieDropdown(goalieInt); + else + setGoalieDropdown(-1); } //////////////// @@ -1146,3 +1171,154 @@ void MainWindow::updateDebugLayers(const LogFrame& frame) { _ui.debugLayers->sortItems(); } } + +/** + * The following methods are event listeners for the manual position assignments. + * In the QT5 event handling system, these methods are automatically connected to the + * QObject with the relevant name due to their naming scheme and their labels as Q_SLOT functions. + * + * For this reason, all of these methods are REQUIRED by the UI - Consolidating them into one method + * produces unnecessary complications. + */ +void MainWindow::on_positionReset_0_clicked() { onResetButtonClicked(0); } + +void MainWindow::on_positionReset_1_clicked() { onResetButtonClicked(1); } + +void MainWindow::on_positionReset_2_clicked() { onResetButtonClicked(2); } + +void MainWindow::on_positionReset_3_clicked() { onResetButtonClicked(3); } + +void MainWindow::on_positionReset_4_clicked() { onResetButtonClicked(4); } + +void MainWindow::on_positionReset_5_clicked() { onResetButtonClicked(5); } + +void MainWindow::on_positionReset_6_clicked() { onResetButtonClicked(6); } + +void MainWindow::on_positionReset_7_clicked() { onResetButtonClicked(7); } + +void MainWindow::on_positionReset_8_clicked() { onResetButtonClicked(8); } + +void MainWindow::on_positionReset_9_clicked() { onResetButtonClicked(9); } + +void MainWindow::on_positionReset_10_clicked() { onResetButtonClicked(10); } + +void MainWindow::on_positionReset_11_clicked() { onResetButtonClicked(11); } + +void MainWindow::on_positionReset_12_clicked() { onResetButtonClicked(12); } + +void MainWindow::on_positionReset_13_clicked() { onResetButtonClicked(13); } + +void MainWindow::on_positionReset_14_clicked() { onResetButtonClicked(14); } + +void MainWindow::on_positionReset_15_clicked() { onResetButtonClicked(15); } + + +void MainWindow::on_robotPosition_0_currentIndexChanged(int value) { + onPositionDropdownChanged(0, value); +} + +void MainWindow::on_robotPosition_1_currentIndexChanged(int value) { + onPositionDropdownChanged(1, value); +} + +void MainWindow::on_robotPosition_2_currentIndexChanged(int value) { + onPositionDropdownChanged(2, value); +} + +void MainWindow::on_robotPosition_3_currentIndexChanged(int value) { + onPositionDropdownChanged(3, value); +} + +void MainWindow::on_robotPosition_4_currentIndexChanged(int value) { + onPositionDropdownChanged(4, value); +} + +void MainWindow::on_robotPosition_5_currentIndexChanged(int value) { + onPositionDropdownChanged(5, value); +} + +void MainWindow::on_robotPosition_6_currentIndexChanged(int value) { + onPositionDropdownChanged(6, value); +} + +void MainWindow::on_robotPosition_7_currentIndexChanged(int value) { + onPositionDropdownChanged(7, value); +} + +void MainWindow::on_robotPosition_8_currentIndexChanged(int value) { + onPositionDropdownChanged(8, value); +} + +void MainWindow::on_robotPosition_9_currentIndexChanged(int value) { + onPositionDropdownChanged(9, value); +} + +void MainWindow::on_robotPosition_10_currentIndexChanged(int value) { + onPositionDropdownChanged(10, value); +} + +void MainWindow::on_robotPosition_11_currentIndexChanged(int value) { + onPositionDropdownChanged(11, value); +} + +void MainWindow::on_robotPosition_12_currentIndexChanged(int value) { + onPositionDropdownChanged(12, value); +} + +void MainWindow::on_robotPosition_13_currentIndexChanged(int value) { + onPositionDropdownChanged(13, value); +} + +void MainWindow::on_robotPosition_14_currentIndexChanged(int value) { + onPositionDropdownChanged(14, value); +} + +void MainWindow::on_robotPosition_15_currentIndexChanged(int value) { + onPositionDropdownChanged(15, value); +} + +void MainWindow::onResetButtonClicked(int robot) { + rj_msgs::msg::OverridePosition message; + message.robot_id = robot; + message.overriding_position = 0; + + override_play_pub_->publish(message); + position_reset_buttons.at(robot)->setEnabled(false); + robot_pos_selectors.at(robot)->setCurrentIndex(0); +} + +void MainWindow::onPositionDropdownChanged(int robot, int position_number) { + if (current_goalie_num_ != robot) { + rj_msgs::msg::OverridePosition message; + message.robot_id = robot; + message.overriding_position = position_number; + + override_play_pub_->publish(message); + if (position_number != 0) { + position_reset_buttons.at(robot)->setEnabled(true); + } else { + position_reset_buttons.at(robot)->setEnabled(false); + } + } +} + +void MainWindow::setGoalieDropdown(int robot) { + for (int i = 0; i < robot_pos_selectors.size(); ++i) { + if (i != robot) { + robot_pos_selectors.at(i)->setEnabled(true); + } else { + robot_pos_selectors.at(i)->setCurrentIndex(0); + robot_pos_selectors.at(i)->setEnabled(false); + onResetButtonClicked(i); + } + } +} + +void MainWindow::populate_override_position_dropdowns() { + for (int i = 0; i < kNumShells; i++) { + robot_pos_selectors[i]->clear(); + for (int j = 0; j < overriding_position_labels.size(); j++) { + robot_pos_selectors[i]->addItem(QString::fromStdString(overriding_position_labels[j])); + } + } +} diff --git a/soccer/src/soccer/ui/main_window.hpp b/soccer/src/soccer/ui/main_window.hpp index 1bedb1716d..0c5a0e6eaa 100644 --- a/soccer/src/soccer/ui/main_window.hpp +++ b/soccer/src/soccer/ui/main_window.hpp @@ -12,6 +12,7 @@ #include #include +#include #include #include #include @@ -19,6 +20,7 @@ #include "field_view.hpp" #include "game_state.hpp" #include "processor.hpp" +#include "strategy/agent/position/overriding_positions.hpp" #include "ui_MainWindow.h" #include "rc-fshare/rtp.hpp" @@ -78,8 +80,6 @@ class MainWindow : public QMainWindow { void setUseRefChecked(bool use_ref); - rclcpp::Publisher::SharedPtr test_play_pub_; - private Q_SLOTS: void addLayer(int i, const QString& name, bool checked); void updateViews(); @@ -164,6 +164,42 @@ private Q_SLOTS: void on_fastBlue_clicked(); void on_fastYellow_clicked(); + // Robot Position Dropdowns and Reset Buttons + void on_robotPosition_0_currentIndexChanged(int value); + void on_robotPosition_1_currentIndexChanged(int value); + void on_robotPosition_2_currentIndexChanged(int value); + void on_robotPosition_3_currentIndexChanged(int value); + void on_robotPosition_4_currentIndexChanged(int value); + void on_robotPosition_5_currentIndexChanged(int value); + void on_robotPosition_6_currentIndexChanged(int value); + void on_robotPosition_7_currentIndexChanged(int value); + void on_robotPosition_8_currentIndexChanged(int value); + void on_robotPosition_9_currentIndexChanged(int value); + void on_robotPosition_10_currentIndexChanged(int value); + void on_robotPosition_11_currentIndexChanged(int value); + void on_robotPosition_12_currentIndexChanged(int value); + void on_robotPosition_13_currentIndexChanged(int value); + void on_robotPosition_14_currentIndexChanged(int value); + void on_robotPosition_15_currentIndexChanged(int value); + + void on_positionReset_0_clicked(); + void on_positionReset_1_clicked(); + void on_positionReset_2_clicked(); + void on_positionReset_3_clicked(); + void on_positionReset_4_clicked(); + void on_positionReset_5_clicked(); + void on_positionReset_6_clicked(); + void on_positionReset_7_clicked(); + void on_positionReset_8_clicked(); + void on_positionReset_9_clicked(); + void on_positionReset_10_clicked(); + void on_positionReset_11_clicked(); + void on_positionReset_12_clicked(); + void on_positionReset_13_clicked(); + void on_positionReset_14_clicked(); + void on_positionReset_15_clicked(); + + Q_SIGNALS: // signal used to let widgets that we're viewing a different log frame now int historyLocationChanged(int value); @@ -186,6 +222,8 @@ private Q_SLOTS: Processor* const _processor; bool _has_external_ref; + int current_goalie_num_ {0}; + // Log history, copied from Logger. // This is used by other controls to get log data without having to copy it // again from the Logger. @@ -196,6 +234,25 @@ private Q_SLOTS: // To export a larger amount of data. std::vector> _longHistory{}; + // Arrays containing dropdown and reset button UI objects + std::array robot_pos_selectors{}; + std::array position_reset_buttons{}; + + // Methods to update positions and broadcast those changes + /* + * Sets which dropdown represents the goalie — disabling it and ensuring the previous goalie + * dropdown is re-enabled. + */ + void setGoalieDropdown(int robot); + /* + * Callback when a dropdown is changed (whether by user or program) + */ + void onPositionDropdownChanged(int robot, int position_number); + /* + * Callback when a reset button is clicked + */ + void onResetButtonClicked(int robot); + // Tree items that are not in LogFrame QTreeWidgetItem* _frameNumberItem{}; QTreeWidgetItem* _elapsedTimeItem{}; @@ -241,6 +298,7 @@ private Q_SLOTS: rclcpp::Node::SharedPtr _node; rclcpp::Client::SharedPtr _quick_commands_srv; rclcpp::Client::SharedPtr _set_game_settings; + rclcpp::Publisher::SharedPtr override_play_pub_; rclcpp::executors::SingleThreadedExecutor _executor; std::thread _executor_thread; @@ -256,4 +314,12 @@ private Q_SLOTS: rj_msgs::msg::GameSettings _game_settings; bool _game_settings_valid = false; + + std::vector overriding_position_labels{"Auto", "Offense", + "Defense", "Free Kicker", + "Penalty Player", "Penalty Non-Kicker", + "Solo Offense", "Smart Idle", + "Zoner", "Idle"}; + + void populate_override_position_dropdowns(); }; diff --git a/soccer/src/soccer/ui/qt/MainWindow.ui b/soccer/src/soccer/ui/qt/MainWindow.ui index 72410177c3..7f63b4c2db 100644 --- a/soccer/src/soccer/ui/qt/MainWindow.ui +++ b/soccer/src/soccer/ui/qt/MainWindow.ui @@ -1962,6 +1962,1171 @@ + + + Positions + + + + + -11 + -1 + 371 + 671 + + + + + 11 + + + 11 + + + 11 + + + 11 + + + + + Robot 0 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 1 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 2 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 3 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 4 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 5 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 6 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 7 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 8 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 9 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 10 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 11 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 12 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 13 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 14 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + + + Robot 15 + + + + + + + Auto + + + 0 + + + + Auto + + + + + Offense + + + + + Defense + + + + + Goal Kicker + + + + + Marker + + + + + Penalty Player + + + + + Seeker + + + + + Waller + + + + + + + + false + + + Reset + + + false + + + + + + +