Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Code Style On test-mode #2264

Open
wants to merge 1 commit into
base: test-mode
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 22 additions & 22 deletions soccer/src/soccer/strategy/agent/position/overriding_positions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,26 @@
#include <string>
#include <vector>

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 {
/*
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
} // namespace Strategy
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,11 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id, "RobotFact
current_position_ = std::make_unique<Defense>(robot_id_);
}

std::string node_name {"robot_factory_position_"};
std::string node_name{"robot_factory_position_"};
_node = std::make_shared<rclcpp::Node>(node_name.append(std::to_string(robot_id_)));
test_play_sub_ = _node->create_subscription<rj_msgs::msg::OverridePosition>(
"override_position_for_robot", 1, [this](const rj_msgs::msg::OverridePosition::SharedPtr msg){test_play_callback(msg);}
);
"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(); });
}
Expand Down Expand Up @@ -127,10 +127,10 @@ void RobotFactoryPosition::handle_ready() {
}

void RobotFactoryPosition::update_position() {
//if (test_play_position_ == Strategy::OverridingPositions::OFFENSE) {
// SPDLOG_INFO("OFFENSE SELECTED");
//set_current_position<Offense>();
//return;
// if (test_play_position_ == Strategy::OverridingPositions::OFFENSE) {
// SPDLOG_INFO("OFFENSE SELECTED");
// set_current_position<Offense>();
// return;
//}
bool manual_position_set = check_for_position_override();
if (manual_position_set) return;
Expand Down Expand Up @@ -354,18 +354,20 @@ 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) {
test_play_position_ = static_cast<Strategy::OverridingPositions>(message->overriding_position);
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) {
test_play_position_ =
static_cast<Strategy::OverridingPositions>(message->overriding_position);
}
}

/**
* Checks test_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 (test_play_position_) {
case Strategy::OverridingPositions::OFFENSE: {
set_current_position<Offense>();
Expand Down Expand Up @@ -398,7 +400,7 @@ bool RobotFactoryPosition::check_for_position_override() {
case Strategy::OverridingPositions::ZONER: {
set_current_position<Zoner>();
return true;
}
}
case Strategy::OverridingPositions::IDLE: {
set_current_position<Idle>();
return true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@
#include <rj_msgs/action/robot_move.hpp>

#include "game_state.hpp"
#include "overriding_positions.hpp"
#include "planning/instant.hpp"
#include "position.hpp"
#include "overriding_positions.hpp"
#include "rj_common/field_dimensions.hpp"
#include "rj_common/time.hpp"
#include "rj_constants/constants.hpp"
Expand Down Expand Up @@ -112,12 +112,12 @@ class RobotFactoryPosition : public Position {
private:
std::unique_ptr<Position> current_position_;

//subscription for test mode - Allows position overriding
// subscription for test mode - Allows position overriding
rclcpp::Subscription<rj_msgs::msg::OverridePosition>::SharedPtr test_play_sub_;
rclcpp::executors::SingleThreadedExecutor _executor;
std::thread _executor_thread;
void test_play_callback(const rj_msgs::msg::OverridePosition::SharedPtr message);
Strategy::OverridingPositions test_play_position_ {Strategy::OverridingPositions::AUTO};
Strategy::OverridingPositions test_play_position_{Strategy::OverridingPositions::AUTO};
rclcpp::Node::SharedPtr _node;

std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
Expand Down
25 changes: 13 additions & 12 deletions soccer/src/soccer/ui/main_window.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -187,16 +187,18 @@ MainWindow::MainWindow(Processor* processor, bool has_external_ref, QWidget* par
_set_game_settings = _node->create_client<rj_msgs::srv::SetGameSettings>(
config_server::topics::kGameSettingsSrv);

test_play_pub_ = _node->create_publisher<rj_msgs::msg::OverridePosition>(
"override_position_for_robot", 1);
test_play_pub_ =
_node->create_publisher<rj_msgs::msg::OverridePosition>("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<QComboBox*>(QString::fromStdString("robotPosition_" + std::to_string(i)));
position_reset_buttons[i] = findChild<QPushButton*>(QString::fromStdString("positionReset_" + std::to_string(i)));
robot_pos_selectors[i] =
findChild<QComboBox*>(QString::fromStdString("robotPosition_" + std::to_string(i)));
position_reset_buttons[i] =
findChild<QPushButton*>(QString::fromStdString("positionReset_" + std::to_string(i)));
}
}

Expand Down Expand Up @@ -1046,14 +1048,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);
//This is a convoluted scheme to figure out who the goalie is
//May not be necessary later
// This is a convoluted scheme to figure out who the goalie is
// May not be necessary later
QString goalieNum = _ui.goalieID->currentText();
bool goalieNumIsInt {false};
bool goalieNumIsInt{false};
int goalieInt = goalieNum.toInt(&goalieNumIsInt);
if (goalieNumIsInt)
setGoalieDropdown(goalieInt);
else
else
setGoalieDropdown(-1);
}

Expand Down Expand Up @@ -1280,10 +1282,10 @@ void MainWindow::onResetButtonClicked(int robot) {
}

void MainWindow::onPositionDropdownChanged(int robot, int position_number) {
//This is a convoluted scheme to figure out who the goalie is
//May not be necessary later
// This is a convoluted scheme to figure out who the goalie is
// May not be necessary later
QString goalieNum = _ui.goalieID->currentText();
bool goalieNumIsInt {false};
bool goalieNumIsInt{false};
int goalieInt = goalieNum.toInt(&goalieNumIsInt);
if (!goalieNumIsInt || goalieInt != robot) {
rj_msgs::msg::OverridePosition message;
Expand Down Expand Up @@ -1318,4 +1320,3 @@ void MainWindow::populate_override_position_dropdowns() {
}
}
}

20 changes: 6 additions & 14 deletions soccer/src/soccer/ui/main_window.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,16 @@
#include <QComboBox>
#include <QMainWindow>
#include <QPushButton>
#include <QComboBox>
#include <QTime>
#include <QTimer>
#include <QtGui/QStandardItemModel>
#include <rclcpp/rclcpp.hpp>

#include <rj_convert/ros_convert.hpp>
#include <rj_msgs/msg/override_position.hpp>
#include <rj_msgs/srv/quick_commands.hpp>
#include <rj_msgs/srv/quick_restart.hpp>
#include <rj_msgs/srv/set_game_settings.hpp>
#include <rj_msgs/msg/override_position.hpp>

#include "field_view.hpp"
#include "game_state.hpp"
Expand Down Expand Up @@ -314,18 +313,11 @@ private Q_SLOTS:
rj_msgs::msg::GameSettings _game_settings;
bool _game_settings_valid = false;

std::vector<std::string> overriding_position_labels {
"Auto",
"Offense",
"Defense",
"Free Kicker",
"Penalty Player",
"Penalty Non-Kicker",
"Solo Offense",
"Smart Idle",
"Zoner",
"Idle"
};
std::vector<std::string> overriding_position_labels{"Auto", "Offense",
"Defense", "Free Kicker",
"Penalty Player", "Penalty Non-Kicker",
"Solo Offense", "Smart Idle",
"Zoner", "Idle"};

void populate_override_position_dropdowns();
};