Test play improvements

config/plays.txt

@@ -0,0 +1,8 @@
+defense.Defense()
+offense.Offense()
+line_up.LineUp()
+keepaway.Keepaway()
+kickoff_play.PrepareKickoff()
+kickoff_play.Kickoff()
+kickoff_play.DefendKickoff()
+None

rj_gameplay/rj_gameplay/gameplay_node.py

@@ -6,7 +6,7 @@
 and allows the PlaySelector to be changed between Test and other forms.
 """
 
-from typing import List, Optional, Tuple
+from typing import List, Optional, Set
 
 import numpy as np
 import rclpy
@@ -16,7 +16,9 @@
 import stp.situation as situation
 import stp.skill
 import stp.utils.world_state_converter as conv
+from rcl_interfaces.msg import SetParametersResult
 from rclpy.node import Node
+from rclpy.parameter import Parameter
 from rclpy.qos import QoSProfile
 from rj_geometry_msgs import msg as geo_msg
 from rj_msgs import msg
@@ -48,18 +50,28 @@ class GameplayNode(Node):
     def __init__(
         self,
         play_selector: situation.IPlaySelector,
-        test_play: Optional[stp.play.Play] = None,
         # TODO: see whether or not world_state is ever not None here
         world_state: Optional[rc.WorldState] = None,
     ) -> None:
         rclpy.init()
         super().__init__("gameplay_node")
 
-        # do not change this line, change the test play passed in at bottom of file
-        self._curr_play = test_play
+        self._test_play = None
+        self._curr_play = None
         self._curr_situation = None
-        # force test play to overwrite play selector if given
-        self._using_test_play = test_play is not None
+
+        file = open("config/plays.txt")
+        self.plays: Set = set(file.read().splitlines())
+        file.close()
+
+        self.declare_parameter("test_play", "None")
+
+        self.add_on_set_parameters_callback(self.update_test_play)
+
+        """uncomment the below line and use the local param server
+        if we need a more robust param setup.
+        Right now this is overengineering though:"""
+        # local_parameters.register_parameters(self)
 
         self.world_state_sub = self.create_subscription(
             msg.WorldState,
@@ -131,7 +143,6 @@ def __init__(
         self.global_parameter_client = GlobalParameterClient(
             self, "global_parameter_server"
         )
-        local_parameters.register_parameters(self)
 
         # publish def_area_obstacles, global obstacles
         self.def_area_obstacles_pub = self.create_publisher(
@@ -147,6 +158,11 @@ def __init__(
         self.debug_text_pub = self.create_publisher(
             StringMsg, "/gameplay/debug_text", 10
         )
+
+        self.test_play_sub = self.create_subscription(
+            StringMsg, "test_play", self.set_test_play_callback, 1
+        )
+
         self.play_selector: situation.IPlaySelector = play_selector
 
     def set_play_state(self, play_state: msg.PlayState):
@@ -235,8 +251,13 @@ def gameplay_tick(self) -> None:
         """
         self.update_world_state()
 
+        if str(self.get_parameter("test_play").value) != "None" or "{0}.{1}".format(
+            self._test_play.__class__.__module__, self._test_play.__class__.__name__
+        ):
+            self._test_play = eval(str(self.get_parameter("test_play").value))
+
         if self.world_state is not None:
-            if not self._using_test_play:
+            if self._test_play is None:
                 new_situation, new_play = self.play_selector.select(self.world_state)
 
                 # if play/situation hasn't changed, keep old play
@@ -245,6 +266,8 @@ def gameplay_tick(self) -> None:
                 ) != type(new_situation):
                     self._curr_play = new_play
                     self._curr_situation = new_situation
+            else:
+                self._curr_play = self._test_play
 
             intents = self._curr_play.tick(self.world_state)
 
@@ -479,6 +502,29 @@ def tick_override_actions(self, world_state) -> None:
     def clear_override_actions(self) -> None:
         self.override_actions = [None] * NUM_ROBOTS
 
+    def update_test_play(self, parameter_list) -> SetParametersResult:
+        """
+        Checks all gameplay node parameters to see if they are valid.
+        Right now test_play is the only one.
+        It should be a string in the plays set.
+        If we use the local param server later, move this logic there.
+        """
+        rejected_parameters = (
+            param
+            for param in parameter_list
+            if param.name == "test_play"
+            and param.type_ is Parameter.Type.STRING
+            and str(param.value) not in self.plays
+        )
+        was_success = not any(rejected_parameters)
+        return SetParametersResult(successful=(was_success))
+
+    def set_test_play_callback(self, test_play_msg: StringMsg):
+        new_test_play = rclpy.parameter.Parameter(
+            "test_play", Parameter.Type.STRING, test_play_msg.data
+        )
+        self.set_parameters([new_test_play])
+
     def shutdown(self) -> None:
         """
         destroys node
@@ -489,11 +535,5 @@ def shutdown(self) -> None:
 
 def main():
     my_play_selector = play_selector.PlaySelector()
-
-    # change this line to test different plays (set to None if no desired test play)
-
-    # test_play = defense.Defense()
-    test_play = None
-
-    gameplay = GameplayNode(my_play_selector, test_play)
+    gameplay = GameplayNode(my_play_selector)
     rclpy.spin(gameplay)

soccer/src/soccer/ui/main_window.cpp

@@ -1,6 +1,7 @@
 #include "main_window.hpp"
 
 #include <ctime>
+#include <fstream>
 
 #include <QActionGroup>
 #include <QDateTime>
@@ -10,6 +11,7 @@
 #include <QInputDialog>
 #include <QMessageBox>
 #include <QString>
+#include <boost/algorithm/string.hpp>
 #include <google/protobuf/descriptor.h>
 
 #include <rj_common/qt_utils.hpp>
@@ -21,7 +23,9 @@
 
 #include "battery_profile.hpp"
 #include "radio/radio.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "robot_status_widget.hpp"
+#include "std_msgs/msg/string.hpp"
 
 #include "rc-fshare/git_version.hpp"
 
@@ -186,6 +190,22 @@ MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* par
         _node->create_client<rj_msgs::srv::QuickCommands>(referee::topics::kQuickCommandsSrv);
     _set_game_settings = _node->create_client<rj_msgs::srv::SetGameSettings>(
         config_server::topics::kGameSettingsSrv);
+
+    // test play logic initialization
+    test_play_pub_ = _node->create_publisher<std_msgs::msg::String>("test_play", 1);
+
+    std::fstream plays;
+    plays.open("config/plays.txt",
+               ios::in);    // open a file to perform read operation using file object
+    if (plays.is_open()) {  // checking whether the file is open
+        std::string to_add;
+        while (getline(plays, to_add)) {  // read data from file object and put it into string.
+            boost::trim(to_add);
+            new QListWidgetItem(tr(to_add.c_str()), _ui.selectedTestsTable);
+        }
+        plays.close();  // close the file object.
+    }
+
     _executor.add_node(_node);
     _executor_thread = std::thread([this]() { _executor.spin(); });
 }
@@ -751,14 +771,6 @@ void MainWindow::status(const QString& text, MainWindow::StatusType status) {
     }
 }
 
-void MainWindow::playIndicatorStatus(bool color) {
-    if (color) {
-        _ui.playIndicatorStatus->setStyleSheet("background-color: #00ff00");
-    } else {
-        _ui.playIndicatorStatus->setStyleSheet("background-color: #ff0000");
-    }
-}
-
 void MainWindow::updateRadioBaseStatus(bool usbRadio) {
     QString label = QString(usbRadio ? "Radio Connected" : "Radio Disconnected");
     if (_ui.radioBaseStatus->text() != label) {
@@ -1170,3 +1182,29 @@ void MainWindow::updateDebugLayers(const LogFrame& frame) {
         _ui.debugLayers->sortItems();
     }
 }
+
+void MainWindow::on_addToTable_clicked() {
+    auto to_add = (_ui.testInput->toPlainText().toStdString());
+    boost::trim(to_add);
+
+    // do not add same test multiple times
+    for (int i = 0; i < _ui.selectedTestsTable->count(); ++i) {
+        auto test = _ui.selectedTestsTable->item(i);
+        auto test_name = test->text().toStdString();
+        if (to_add == test_name) {
+            return;
+        }
+    }
+
+    new QListWidgetItem(tr(to_add.c_str()), _ui.selectedTestsTable);
+}
+
+void MainWindow::on_testRun_clicked() {
+    if (_ui.selectedTestsTable->currentItem() == nullptr) {
+        return;
+    }
+    std::string test_name = _ui.selectedTestsTable->currentItem()->text().toStdString();
+    auto message = std_msgs::msg::String();
+    message.data = test_name;
+    test_play_pub_->publish(message);
+}

soccer/src/soccer/ui/main_window.hpp

@@ -73,6 +73,8 @@ class MainWindow : public QMainWindow {
 
     void setUseRefChecked(bool use_ref);
 
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr test_play_pub_;
+
 private Q_SLOTS:
     void addLayer(int i, const QString& name, bool checked);
     void updateViews();
@@ -147,8 +149,6 @@ private Q_SLOTS:
     void on_debugLayers_itemChanged(QListWidgetItem* item);
     void on_debugLayers_customContextMenuRequested(const QPoint& pos);
 
-    void playIndicatorStatus(bool color);
-
     // Fast Ref Buttons
     void on_fastHalt_clicked();
     void on_fastStop_clicked();
@@ -161,6 +161,10 @@ private Q_SLOTS:
     void on_fastIndirectBlue_clicked();
     void on_fastIndirectYellow_clicked();
 
+    // Testing
+    void on_addToTable_clicked();
+    void on_testRun_clicked();
+
 Q_SIGNALS:
     // signal used to let widgets that we're viewing a different log frame now
     int historyLocationChanged(int value);