From 03268543a0c12f2cf1d9e2e82cff623e41003c4d Mon Sep 17 00:00:00 2001 From: Paulo Grego <12087043+Pogrego@users.noreply.github.com> Date: Tue, 31 Aug 2021 12:54:07 -0300 Subject: [PATCH 1/2] Comentarios na Game.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Comentários explicando melhor o que cada coisa faz no código --- src/strategy/Game.cpp | 96 ++++++++++++++++++++++++++----------------- 1 file changed, 58 insertions(+), 38 deletions(-) diff --git a/src/strategy/Game.cpp b/src/strategy/Game.cpp index 7e317e4..8800696 100644 --- a/src/strategy/Game.cpp +++ b/src/strategy/Game.cpp @@ -8,12 +8,12 @@ Game::Game(int argc, char *argv[]) this->startup(argc, argv); } -/** +/* ********************************************** * @brief Set network config and team color * * @param argc * @param argv - */ + * *********************************************/ void Game::startup(int argc, char **argv) { ArgParse::ArgumentParser parser(argv[0], "GER VSSS FIRASim strategy server"); @@ -50,24 +50,39 @@ void Game::startup(int argc, char **argv) << "Color team: " << (this->is_yellow ? "yellow":"blue") << std::endl; } -//There's no need to invert after assignement. This doensn't need to be so complex +/* ********************************************************* + * @brief takes the ball's positions x and y, + * as well as its vx and vy velocities + * and multiply both of them by (-1) + * + * @obs There's no need to invert after assignement. + * This doensn't need to be so complex + * *********************************************************/ void Game::invert_ball() { - ball.set_x(-ball.x()); - ball.set_y(-ball.y()); - ball.set_vx(-ball.vx()); - ball.set_vy(-ball.vy()); + ball.set_x(-ball.x()); // invert positon x + ball.set_y(-ball.y()); // invert positon y + ball.set_vx(-ball.vx()); // invert speed vx + ball.set_vy(-ball.vy()); // invert speed vy } -//There's no need to invert after assignement. This doensn't need to be so complex +/* ********************************************************* + * @brief takes the robot's positions x and y, + * as well as its vx and vy velocities and its + * orientation on the field and multiply both of + * them by (-1) + * + * @obs There's no need to invert after assignement. + * This doensn't need to be so complex + * *********************************************************/ fira_message::Robot Game::invert_robot(fira_message::Robot &robot) { - robot.set_x(-robot.x()); - robot.set_y(-robot.y()); - robot.set_vx(-robot.vx()); - robot.set_vy(-robot.vy()); - robot.set_orientation(math::wrap_to_pi(robot.orientation() + PI)); - robot.set_vorientation(-robot.vorientation()); + robot.set_x(-robot.x()); // invert positon x + robot.set_y(-robot.y()); // invert positon y + robot.set_vx(-robot.vx()); // invert speed vx + robot.set_vy(-robot.vy()); // invert speed vy + robot.set_orientation(math::wrap_to_pi(robot.orientation() + PI)); // invert orientation + robot.set_vorientation(-robot.vorientation()); // invert speed orientation return robot; } @@ -91,18 +106,23 @@ void Game::invert_field_if_yellow() } } -/** +/* **************************************************************** * @brief returns vector of blue or yellow robots * * @param is_yellow * @param frame * @return vector - */ + * ***************************************************************/ vector Game::detect_robots(bool is_yellow, fira_message::Frame frame) { vector robots; - uint robots_size = is_yellow ? frame.robots_yellow_size() : frame.robots_blue_size(); + +// Ternary command: If the identified robot is yellow, it will mark the robots_size as team yellow. +// Else, it will mark the robots_size as team blue. + uint robots_size = is_yellow ? frame.robots_yellow_size() : frame.robots_blue_size(); robots.resize(robots_size); + +// Runs a for command to assign each robots_size prevviously marked to its respective robot for (uint i = 0; i < robots_size; ++i) { robots[i] = is_yellow ? frame.robots_yellow(i) : frame.robots_blue(i); @@ -111,16 +131,16 @@ vector Game::detect_robots(bool is_yellow, fira_message::Fr return robots; } -/** +/* ***************************************************************************** * @brief stores state of current game objects into Game attributes * * @param frame current game frame - */ + * *****************************************************************************/ void Game::detect_objects(fira_message::Frame &frame) { - this->ball = frame.ball(); - this->my_robots = detect_robots(this->is_yellow, frame); - this->enemy_robots = detect_robots(!this->is_yellow, frame); + this->ball = frame.ball(); // detect ball + this->my_robots = detect_robots(this->is_yellow, frame); // detect my robot + this->enemy_robots = detect_robots(!this->is_yellow, frame); // detect enemy robot // TODO MOVE THIS invert_field_if_yellow(); @@ -128,25 +148,25 @@ void Game::detect_objects(fira_message::Frame &frame) this->robots.insert(robots.end(), enemy_robots.begin(), enemy_robots.end()); } -/** +/* *********************************************************************** * @brief return closest robots index to ball * * @return int - */ + * ************************************************************************/ unsigned int Game::robot_next_to_ball(Attacker &atk, Midfielder &mid) { - double atk_ball_dist = atk.future_dist_to(ball); - double mid_ball_dist = mid.future_dist_to(ball); + double atk_ball_dist = atk.future_dist_to(ball); // distance between ball and attacker + double mid_ball_dist = mid.future_dist_to(ball); // distance between ball and midfielder - return (mid_ball_dist < atk_ball_dist) ? - mid.get_robot().robot_id() : - atk.get_robot().robot_id(); + return (mid_ball_dist < atk_ball_dist) ? // ternary question: + mid.get_robot().robot_id() : // if midfielder is closest, return midfielder's ID + atk.get_robot().robot_id(); // else, attacker is closest, return attacker's ID } -/** +/* ************************************************************************** * @brief switch atk and mid depending on which one is closer to ball * @todo dynamic gkp - */ + * **************************************************************************/ void Game::select_roles(Goalkeeper &gkp, Attacker &atk, Midfielder &mid) { int new_atk_id = robot_next_to_ball(atk, mid); @@ -158,9 +178,9 @@ void Game::select_roles(Goalkeeper &gkp, Attacker &atk, Midfielder &mid) // TODO POLYMORPHISM WITH PLAYER void Game::send_commands(VSSClient &sim_client) { - Goalkeeper gkp(my_robots[0]); - Attacker atk(my_robots[1]); - Midfielder mid(my_robots[2]); + Goalkeeper gkp(my_robots[0]); // goalkeeper is robot number 0 + Attacker atk(my_robots[1]); // attacker is robot number 1 + Midfielder mid(my_robots[2]); // midfielder is robot number 2 // FUNCTION HAS MORE THAN ONE FUNCTIONALITY select_roles(gkp, atk, mid); @@ -169,9 +189,9 @@ void Game::send_commands(VSSClient &sim_client) ctrl::vec2 atk_command = atk.play(this->ball, this->robots); ctrl::vec2 mid_command = mid.play(this->ball, this->robots); - sim_client.sendCommand(gkp.get_robot().robot_id(), gkp_command.x, gkp_command.y); - sim_client.sendCommand(atk.get_robot().robot_id(), atk_command.x, atk_command.y); - sim_client.sendCommand(mid.get_robot().robot_id(), mid_command.x, mid_command.y); + sim_client.sendCommand(gkp.get_robot().robot_id(), gkp_command.x, gkp_command.y); // goalkeeper receives its command from strategy + sim_client.sendCommand(atk.get_robot().robot_id(), atk_command.x, atk_command.y); // attacker receives its command from strategy + sim_client.sendCommand(mid.get_robot().robot_id(), mid_command.x, mid_command.y); // midfielder receives its command from strategy } void Game::run() @@ -186,7 +206,7 @@ void Game::run() fira_message::sim_to_ref::Environment packet; - while (true) + while (true) // this while detect objets in real time and then send commands { if (client.receive(packet) && packet.has_frame()) { From 901c7ec74233fc669b32e628c3ee40c9eca7fa76 Mon Sep 17 00:00:00 2001 From: Paulo Grego <12087043+Pogrego@users.noreply.github.com> Date: Tue, 31 Aug 2021 13:00:29 -0300 Subject: [PATCH 2/2] Update Goalkeeper.cpp --- src/strategy/Goalkeeper.cpp | 246 +++++++++++++++++++++++++++++------- 1 file changed, 201 insertions(+), 45 deletions(-) diff --git a/src/strategy/Goalkeeper.cpp b/src/strategy/Goalkeeper.cpp index 1dd4d98..8800696 100644 --- a/src/strategy/Goalkeeper.cpp +++ b/src/strategy/Goalkeeper.cpp @@ -1,64 +1,220 @@ -#include "Goalkeeper.h" +#include +#include "strategy/Game.h" +#include "util/argparse.h" +#include "net/referee_client.h" -Goalkeeper::Goalkeeper(fira_message::Robot &robot): Player(robot) {} +Game::Game(int argc, char *argv[]) +{ + this->startup(argc, argv); +} -unsigned int Goalkeeper::lock_count = 0; +/* ********************************************** + * @brief Set network config and team color + * + * @param argc + * @param argv + * *********************************************/ +void Game::startup(int argc, char **argv) +{ + ArgParse::ArgumentParser parser(argv[0], "GER VSSS FIRASim strategy server"); + parser.add_argument('m', "multicast_ip", "Vision and Referee IP", ArgParse::value("224.0.0.1")); + parser.add_argument('c', "command_ip", "Command IP", ArgParse::value("127.0.0.1")); + parser.add_argument('d', "command_port", "Command port", ArgParse::value(20011)); + parser.add_argument('e', "referee_port", "Referee foul port", ArgParse::value(10003)); + parser.add_argument('r', "replacer_port", "Referee command port", ArgParse::value(10004)); + parser.add_argument('v', "vision_port", "Vision port", ArgParse::value(10002)); + parser.add_argument('t', "team_yellow", "Team collor yellow (true/false)", ArgParse::value(false)); + parser.add_argument('h', "help", "Show help menu"); -/** - * @brief defend our goal by following ball y projection using a vertical sigmoid univector field + auto args = parser.parse_args(argc, argv); + + if (args["help"]->as()) + { + std::cout << parser.help() << std::endl; + exit(0); + } + + this->conf.multicast_ip = args["multicast_ip"]->as(); + this->conf.command_ip = args["command_ip"]->as(); + this->conf.command_port = args["command_port"]->as(); + this->conf.referee_port = args["referee_port"]->as(); + this->conf.replacer_port = args["replacer_port"]->as(); + this->conf.vision_port = args["vision_port"]->as(); + this->is_yellow = args["team_yellow"]->as(); + + // Print startup configuration + std::cout << "Vision server at " << this->conf.multicast_ip << ":" << this->conf.vision_port << std::endl + << "Command listen to " << this->conf.command_ip << ":" << this->conf.command_port << std::endl + << "Referee server at " << this->conf.multicast_ip << ":" << this->conf.referee_port << std::endl + << "Replacer listen to " << this->conf.multicast_ip << ":" << this->conf.replacer_port << std::endl + << "Color team: " << (this->is_yellow ? "yellow":"blue") << std::endl; +} + +/* ********************************************************* + * @brief takes the ball's positions x and y, + * as well as its vx and vy velocities + * and multiply both of them by (-1) * - * @param ball_pos predicted position of ball on current frame - * @return ctrl::vec2 - */ -ctrl::vec2 Goalkeeper::defend_goal_from(ctrl::vec2 ball_pos) + * @obs There's no need to invert after assignement. + * This doensn't need to be so complex + * *********************************************************/ +void Game::invert_ball() { - ctrl::vec2 target, univec; - double y_target, phi; - - // if ball is too close, goalkeeper can get out of y limit to reach ball - if (ball_pos.x < Goalkeeper::X_LIMIT_BALL) - y_target = ball_pos.y; - else - y_target = math::bound(ball_pos.y, -Goalkeeper::Y_LIMIT_GKP, Goalkeeper::Y_LIMIT_GKP); + ball.set_x(-ball.x()); // invert positon x + ball.set_y(-ball.y()); // invert positon y + ball.set_vx(-ball.vx()); // invert speed vx + ball.set_vy(-ball.vy()); // invert speed vy +} + +/* ********************************************************* + * @brief takes the robot's positions x and y, + * as well as its vx and vy velocities and its + * orientation on the field and multiply both of + * them by (-1) + * + * @obs There's no need to invert after assignement. + * This doensn't need to be so complex + * *********************************************************/ +fira_message::Robot Game::invert_robot(fira_message::Robot &robot) +{ + robot.set_x(-robot.x()); // invert positon x + robot.set_y(-robot.y()); // invert positon y + robot.set_vx(-robot.vx()); // invert speed vx + robot.set_vy(-robot.vy()); // invert speed vy + robot.set_orientation(math::wrap_to_pi(robot.orientation() + PI)); // invert orientation + robot.set_vorientation(-robot.vorientation()); // invert speed orientation - // if ball is behind goalkeeper x limit, he follows ball x position - target = ctrl::vec2(std::min(Goalkeeper::X_LIMIT_GKP, ball_pos.x), y_target); - phi = this->univec_vertical_sigmoid_field(target); + return robot; +} - sincos(phi, &univec.y, &univec.x); +//There's no need to invert after assignement. This doensn't need to be so complex +void Game::invert_field_if_yellow() +{ + if (is_yellow) + { + invert_ball(); - return univec; + // DOES THIS REALLY NEED TO USE LAMBDA + std::for_each(my_robots.begin(), my_robots.end(), + [this](fira_message::Robot &robot) { + robot = invert_robot(robot); + }); + std::for_each(enemy_robots.begin(), enemy_robots.end(), + [this](fira_message::Robot &robot) { + robot = invert_robot(robot); + }); + } } -/** - * @brief returns goalkeeper final motors speed +/* **************************************************************** + * @brief returns vector of blue or yellow robots * - * @param ball - * @return ctrl::vec2 - */ -ctrl::vec2 Goalkeeper::play(fira_message::Ball &ball) + * @param is_yellow + * @param frame + * @return vector + * ***************************************************************/ +vector Game::detect_robots(bool is_yellow, fira_message::Frame frame) { - //handle robot locked - if (is_locked(this->lock_count)) + vector robots; + +// Ternary command: If the identified robot is yellow, it will mark the robots_size as team yellow. +// Else, it will mark the robots_size as team blue. + uint robots_size = is_yellow ? frame.robots_yellow_size() : frame.robots_blue_size(); + robots.resize(robots_size); + +// Runs a for command to assign each robots_size prevviously marked to its respective robot + for (uint i = 0; i < robots_size; ++i) { - bool cw = this->robot.y() < ball.y(); - return spin(cw); + robots[i] = is_yellow ? frame.robots_yellow(i) : frame.robots_blue(i); } + + return robots; +} + +/* ***************************************************************************** + * @brief stores state of current game objects into Game attributes + * + * @param frame current game frame + * *****************************************************************************/ +void Game::detect_objects(fira_message::Frame &frame) +{ + this->ball = frame.ball(); // detect ball + this->my_robots = detect_robots(this->is_yellow, frame); // detect my robot + this->enemy_robots = detect_robots(!this->is_yellow, frame); // detect enemy robot + + // TODO MOVE THIS + invert_field_if_yellow(); + this->robots = my_robots; + this->robots.insert(robots.end(), enemy_robots.begin(), enemy_robots.end()); +} + +/* *********************************************************************** + * @brief return closest robots index to ball + * + * @return int + * ************************************************************************/ +unsigned int Game::robot_next_to_ball(Attacker &atk, Midfielder &mid) +{ + double atk_ball_dist = atk.future_dist_to(ball); // distance between ball and attacker + double mid_ball_dist = mid.future_dist_to(ball); // distance between ball and midfielder + + return (mid_ball_dist < atk_ball_dist) ? // ternary question: + mid.get_robot().robot_id() : // if midfielder is closest, return midfielder's ID + atk.get_robot().robot_id(); // else, attacker is closest, return attacker's ID +} + +/* ************************************************************************** + * @brief switch atk and mid depending on which one is closer to ball + * @todo dynamic gkp + * **************************************************************************/ +void Game::select_roles(Goalkeeper &gkp, Attacker &atk, Midfielder &mid) +{ + int new_atk_id = robot_next_to_ball(atk, mid); + gkp.set_robot(my_robots[0]); // gkp is always robot 0 + atk.set_robot(my_robots[new_atk_id]); + mid.set_robot(my_robots[3 - new_atk_id]); +} + +// TODO POLYMORPHISM WITH PLAYER +void Game::send_commands(VSSClient &sim_client) +{ + Goalkeeper gkp(my_robots[0]); // goalkeeper is robot number 0 + Attacker atk(my_robots[1]); // attacker is robot number 1 + Midfielder mid(my_robots[2]); // midfielder is robot number 2 + + // FUNCTION HAS MORE THAN ONE FUNCTIONALITY + select_roles(gkp, atk, mid); - ctrl::vec2 univec, motors_speeds, ball_fut_pos; + ctrl::vec2 gkp_command = gkp.play(this->ball); + ctrl::vec2 atk_command = atk.play(this->ball, this->robots); + ctrl::vec2 mid_command = mid.play(this->ball, this->robots); - ball_fut_pos = this->future_position_relative_to(ball, Goalkeeper::DT_GKP); + sim_client.sendCommand(gkp.get_robot().robot_id(), gkp_command.x, gkp_command.y); // goalkeeper receives its command from strategy + sim_client.sendCommand(atk.get_robot().robot_id(), atk_command.x, atk_command.y); // attacker receives its command from strategy + sim_client.sendCommand(mid.get_robot().robot_id(), mid_command.x, mid_command.y); // midfielder receives its command from strategy +} - //if ball is close enought to the goalkeeper, we should spin in order to kick it - if (this->get_pos().distance(ball_fut_pos) <= Goalkeeper::KICK_DIST) - { - motors_speeds = this->spin((this->robot.y() < ball_fut_pos.y)); // kick ball - } - else +void Game::run() +{ + RoboCupSSLClient client(this->conf.vision_port, this->conf.multicast_ip); + VSSClient sim_client(this->conf.command_port, this->conf.command_ip, this->is_yellow); + + // TODO: REFEREE + RefereeClient referee(this->conf.referee_port, this->conf.replacer_port, this->conf.multicast_ip); + + client.open(false); // opens client + + fira_message::sim_to_ref::Environment packet; + + while (true) // this while detect objets in real time and then send commands { - univec = this->defend_goal_from(ball_fut_pos); - motors_speeds = this->move(univec); - } + if (client.receive(packet) && packet.has_frame()) + { + fira_message::Frame detection = packet.frame(); - return motors_speeds; -} \ No newline at end of file + detect_objects(detection); + + send_commands(sim_client); + } + } +}