Merge pull request #119 from RoboTeamTwente/BallPlacementUs

Fully include next ref command

CMakeLists.txt

@@ -53,7 +53,7 @@ endif ()
 set(DEBUG_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-g" "-O0" "-Wall" "-Wextra" "-Wnon-virtual-dtor" "-pedantic" "-fdiagnostics-color=always")
 
 # march mtune https://stackoverflow.com/questions/54039176/mtune-and-march-when-compiling-in-a-docker-image
-set(RELEASE_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-Ofast" "-march=x86-64" "-mtune=generic" "-fdiagnostics-color=always")
+set(RELEASE_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-O3" "-march=native" "-fdiagnostics-color=always")
 
 # Specify manually which compiler arguments we want to use, either DEBUG (default) or RELEASE ones
 # If you want to build in release mode, pass '-DCMAKE_BUILD_TYPE=RELEASE' as argument to cmake

roboteam_ai/CMakeLists.txt

@@ -119,7 +119,8 @@ add_library(roboteam_ai_plays
         # referee_specific/
         ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/AggressiveStopFormation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementThem.cpp
-        ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUs.cpp
+        ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp
+        ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/DefensiveStopFormation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/FreeKickThem.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp
@@ -157,6 +158,7 @@ add_library(roboteam_ai_evaluation
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp
+        ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp
         ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp

roboteam_ai/include/roboteam_ai/control/ControlModule.h

@@ -30,7 +30,7 @@ class ControlModule {
      * @param command Robot command that needs to be checked
      * @param robot Info about the robot
      */
-    static void limitRobotCommand(rtt::RobotCommand& command, std::optional<rtt::world::view::RobotView> robot);
+    static void limitRobotCommand(rtt::RobotCommand& command, rtt::world::view::RobotView robot);
 
     /**
      * @brief Limits the velocity with a control_constants value
@@ -43,7 +43,7 @@ class ControlModule {
      * @param command Robot command that needs to be checked
      * @param robot Info about the robot
      */
-    static void limitAngularVel(rtt::RobotCommand& command, std::optional<rtt::world::view::RobotView> robot);
+    static void limitAngularVel(rtt::RobotCommand& command, rtt::world::view::RobotView robot);
 
     /**
      * @brief Rotates the robot command to the other side of the field
@@ -57,7 +57,7 @@ class ControlModule {
      * @param command Robot command that needs to be added
      * @param robot Info about the robot
      */
-    static void addRobotCommand(std::optional<rtt::world::view::RobotView> robot, const rtt::RobotCommand& command) noexcept;
+    static void addRobotCommand(std::optional<rtt::world::view::RobotView> robot, rtt::RobotCommand command) noexcept;
     /**
      * @brief Sends all commands to robothub
      */

roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h

@@ -17,6 +17,7 @@ enum class GlobalEvaluation {
     /// Game States
     BallPlacementThemGameState = 0,
     BallPlacementUsGameState,
+    BallPlacementUsDirectGameState,
     FreeKickThemGameState,
     FreeKickUsGameState,
     HaltGameState,

roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h

@@ -0,0 +1,32 @@
+//
+// Created by timovdk on 4/14/20.
+//
+
+#ifndef RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H
+#define RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H
+
+#include "stp/evaluations/BaseEvaluation.h"
+
+namespace rtt::ai::stp::evaluation {
+/**
+ * @brief Class that evaluates the ball placement us game state when the next command is direct free kick
+ */
+class BallPlacementUsDirectGameStateEvaluation : public BaseEvaluation {
+   public:
+    /**
+     * @brief Calculates the score for the ball placement us game state when the next command is direct free kick
+     * @param world The current world
+     * @param field The current field
+     * @return The score of the ball placement direct us game state
+     */
+    [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override;
+
+    /**
+     * @brief Retrieves the name of the evaluation
+     * @return A string containing the name of the evaluation
+     */
+    const char* getName() override { return "gs::BallPlacementUsDirect"; }
+};
+}  // namespace rtt::ai::stp::evaluation
+
+#endif  // RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H

roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsForceStart.h

@@ -0,0 +1,45 @@
+#ifndef RTT_BallPlacementUsForceStart_H
+#define RTT_BallPlacementUsForceStart_H
+
+#include "stp/Play.hpp"
+
+namespace rtt::ai::stp::play {
+
+/**
+ * @brief The ball placement us Force Start play is executed when the ball placement us game state is selected and the next ref command is not Free kick us, meaning it will be a
+ * force start
+ */
+class BallPlacementUsForceStart : public Play {
+   public:
+    /**
+     * @brief Constructor that initializes roles with roles that are necessary for this play
+     */
+    BallPlacementUsForceStart();
+
+    /**
+     * @brief Calculates the score of this play to determine which play is best in this situation
+     * @param field The current Field
+     * @return The score of this play (0-255)
+     */
+    uint8_t score(const rtt::Field& field) noexcept override;
+
+    /**
+     * @brief Assigns robots to roles of this play
+     * @return A map with assigned roles
+     */
+    Dealer::FlagMap decideRoleFlags() const noexcept override;
+
+    /**
+     * @brief Calculates info for the roles
+     */
+    void calculateInfoForRoles() noexcept override;
+
+    /**
+     * @brief Retrieves the name of the play
+     * @return The name of the play as a string
+     */
+    const char* getName() const override;
+};
+}  // namespace rtt::ai::stp::play
+
+#endif  // RTT_BallPlacementUsForceStart_H

roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUs.h → roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsFreeKick.h

@@ -1,19 +1,19 @@
-#ifndef RTT_BALLPLACEMENTUS_H
-#define RTT_BALLPLACEMENTUS_H
+#ifndef RTT_BallPlacementUsFreeKick_H
+#define RTT_BallPlacementUsFreeKick_H
 
 #include "stp/Play.hpp"
 
 namespace rtt::ai::stp::play {
 
 /**
- * @brief The ball placement us play is executed when the ball placement us game state is selected
+ * @brief The ball placement us Free kick play is executed when the ball placement us game state is selected and the next ref command is Free Kick Us
  */
-class BallPlacementUs : public Play {
+class BallPlacementUsFreeKick : public Play {
    public:
     /**
      * @brief Constructor that initializes roles with roles that are necessary for this play
      */
-    BallPlacementUs();
+    BallPlacementUsFreeKick();
 
     /**
      * @brief Calculates the score of this play to determine which play is best in this situation
@@ -41,4 +41,4 @@ class BallPlacementUs : public Play {
 };
 }  // namespace rtt::ai::stp::play
 
-#endif  // RTT_BALLPLACEMENTUS_H
+#endif  // RTT_BallPlacementUsFreeKick_H

roboteam_ai/include/roboteam_ai/utilities/RefCommand.h

@@ -37,6 +37,8 @@ enum class RefCommand {
     PENALTY_US = 20,
     PENALTY_THEM = 21,
     BALL_PLACEMENT_US_DIRECT = 22,  // Ball placement before a direct free kick
+    DIRECT_FREE_US_STOP = 23,       // Direct free kick us directly after a stop
+    DIRECT_FREE_THEM_STOP = 24,     // Direct free kick them directly after a stop
 
     UNDEFINED = -1
 };
@@ -80,6 +82,12 @@ inline std::ostream &operator<<(std::ostream &os, const RefCommand &command) {
         case RefCommand::DIRECT_FREE_THEM:
             os << "DIRECT_FREE_THEM";
             break;
+        case RefCommand::DIRECT_FREE_US_STOP:
+            os << "DIRECT_FREE_US_STOP";
+            break;
+        case RefCommand::DIRECT_FREE_THEM_STOP:
+            os << "DIRECT_FREE_THEM_STOP";
+            break;
         case RefCommand::TIMEOUT_US:
             os << "TIMEOUT_US";
             break;

roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h

@@ -67,6 +67,8 @@ class StrategyManager {
         GameState(RefCommand::BALL_PLACEMENT_THEM, Constants::RULESET_STOP()),
         GameState(RefCommand::BALL_PLACEMENT_US, Constants::RULESET_STOP()),
         GameState(RefCommand::BALL_PLACEMENT_US_DIRECT, Constants::RULESET_STOP()),
+        GameState(RefCommand::DIRECT_FREE_THEM_STOP, Constants::RULESET_STOP()),
+        GameState(RefCommand::DIRECT_FREE_US_STOP, Constants::RULESET_STOP()),
         GameState(RefCommand::PREPARE_KICKOFF_US, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_US),
         GameState(RefCommand::PREPARE_KICKOFF_THEM, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_THEM),
         GameState(RefCommand::PREPARE_PENALTY_US, Constants::RULESET_STOP(), false, RefCommand::PENALTY_US),

roboteam_ai/src/STPManager.cpp

@@ -26,7 +26,8 @@
 #include "stp/plays/offensive/ChippingPass.h"
 #include "stp/plays/referee_specific/AggressiveStopFormation.h"
 #include "stp/plays/referee_specific/BallPlacementThem.h"
-#include "stp/plays/referee_specific/BallPlacementUs.h"
+#include "stp/plays/referee_specific/BallPlacementUsForceStart.h"
+#include "stp/plays/referee_specific/BallPlacementUsFreeKick.h"
 #include "stp/plays/referee_specific/DefensiveStopFormation.h"
 #include "stp/plays/referee_specific/FreeKickThem.h"
 #include "stp/plays/referee_specific/FreeKickUsAtGoal.h"
@@ -60,7 +61,8 @@ const STPManager::PlaysVec STPManager::plays = ([] {
     plays.emplace_back(std::make_unique<plays::KeeperKickBall>());
     plays.emplace_back(std::make_unique<plays::DefensiveStopFormation>());
     plays.emplace_back(std::make_unique<plays::AggressiveStopFormation>());
-    plays.emplace_back(std::make_unique<plays::BallPlacementUs>());
+    plays.emplace_back(std::make_unique<plays::BallPlacementUsFreeKick>());
+    plays.emplace_back(std::make_unique<plays::BallPlacementUsForceStart>());
     plays.emplace_back(std::make_unique<plays::BallPlacementThem>());
     // plays.emplace_back(std::make_unique<play::TimeOut>());
     plays.emplace_back(std::make_unique<plays::PenaltyThemPrepare>());

roboteam_ai/src/control/ControlModule.cpp

@@ -19,58 +19,54 @@ void ControlModule::rotateRobotCommand(rtt::RobotCommand& command) {
     command.targetAngle += M_PI;
 }
 
-void ControlModule::limitRobotCommand(rtt::RobotCommand& command, std::optional<rtt::world::view::RobotView> robot) {
+void ControlModule::limitRobotCommand(rtt::RobotCommand& command, rtt::world::view::RobotView robot) {
     limitVel(command);
     limitAngularVel(command, robot);
 }
 
 void ControlModule::limitVel(rtt::RobotCommand& command) {
-    // The robot can currently not reach very low speeds- if we want it to move a non-trivial amount, we need to send a higher velocity than the path-planning outputs
-    // TODO: Look at the correct value for this with control
-    if (command.velocity.length() > 0.03 && command.velocity.length() < 0.25) command.velocity = command.velocity.stretchToLength(0.25);
     command.velocity = command.velocity.stretchToLength(std::clamp(command.velocity.length(), 0.0, Constants::MAX_VEL_CMD()));
 }
 
-void ControlModule::limitAngularVel(rtt::RobotCommand& command, std::optional<rtt::world::view::RobotView> robot) {
+void ControlModule::limitAngularVel(rtt::RobotCommand& command, rtt::world::view::RobotView robot) {
     // Limit the angular velocity when the robot has the ball by setting the target angle in small steps
-    // Might want to limit on the robot itself
-    if (robot.value()->hasBall() && !command.useAngularVelocity) {
+    if (robot->hasBall() && !command.useAngularVelocity) {
         auto targetAngle = command.targetAngle;
-        // TODO: Why use optional robotView if we never check for the case where it does not contain one?
-        auto robotAngle = robot.value()->getAngle();
+        auto robotAngle = robot->getAngle();
 
+        // Adjust robot angle if game is not on the left
         if (!GameSettings::isLeft()) {
             robotAngle += M_PI;
         }
 
-        // If the angle error is larger than the desired angle rate, the angle command is adjusted
+        // If the angle error is larger than the desired angle rate, adjust the angle command
         if (robotAngle.shortestAngleDiff(targetAngle) > stp::control_constants::ANGLE_RATE) {
-            // Direction of rotation is the shortest distance
+            // Determine direction of rotation (shortest distance)
             int direction = Angle(robotAngle).rotateDirection(targetAngle) ? 1 : -1;
+
             // Set the angle command to the current robot angle + the angle rate
             command.targetAngle = robotAngle + Angle(direction * stp::control_constants::ANGLE_RATE);
         }
     }
     // TODO: Well, then also limit the target angular velocity just like target angle!
 }
 
-void ControlModule::addRobotCommand(std::optional<::rtt::world::view::RobotView> robot, const rtt::RobotCommand& command) noexcept {
-    rtt::RobotCommand robot_command = command;  // TODO: Why make a copy of the command? It will be copied anyway when we put it in the vector
+void ControlModule::addRobotCommand(std::optional<::rtt::world::view::RobotView> robot, rtt::RobotCommand command) noexcept {
     // If we are not left, commands should be rotated (because we play as right)
     if (!GameSettings::isLeft()) {
-        rotateRobotCommand(robot_command);
+        rotateRobotCommand(command);
     }
 
-    if (robot) limitRobotCommand(robot_command, robot);
+    if (robot) limitRobotCommand(command, *robot);
 
     // if we are in simulation; adjust w() to be angular velocity)
-    if (GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR && !robot_command.useAngularVelocity) {
-        simulator_angular_control(robot, robot_command);
+    if (GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR && !command.useAngularVelocity) {
+        simulator_angular_control(robot, command);
     }
 
     // Only add commands with a robotID that is not in the vector yet
-    if (robot_command.id >= 0 && robot_command.id < 16) {
-        robotCommands.emplace_back(robot_command);
+    if (command.id >= 0 && command.id < 16) {
+        robotCommands.emplace_back(command);
     }
 }
 
@@ -105,9 +101,13 @@ void ControlModule::simulator_angular_control(const std::optional<::rtt::world::
 }
 
 void ControlModule::sendAllCommands() {
-    // TODO: check for double commands (But do we really have to do that?)
+    // Remove duplicate commands
+    std::sort(robotCommands.begin(), robotCommands.end(), [](const rtt::RobotCommand& a, const rtt::RobotCommand& b) { return a.id < b.id; });
+    auto last = std::unique(robotCommands.begin(), robotCommands.end(), [](const rtt::RobotCommand& a, const rtt::RobotCommand& b) { return a.id == b.id; });
+    robotCommands.erase(last, robotCommands.end());
 
-    io::io.publishAllRobotCommands(robotCommands);  // When vector has all commands, send in one go
+    // When vector has all commands, send in one go
+    io::io.publishAllRobotCommands(robotCommands);
     robotCommands.clear();
 }
 }  // namespace rtt::ai::control

roboteam_ai/src/stp/Play.cpp

@@ -63,7 +63,7 @@ void Play::update() noexcept {
     // If we have more robots than allowed, one drives to the edge of the field
     if (currentMaxRobots < sizeUs) {
         stpInfos[roles[currentMaxRobots]->getName()].setShouldAvoidBall(true);
-        stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0, -field.playArea.width() / 2));
+        stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0.0, -field.playArea.width() / 2));
     }
 
     // Loop through roles and update them if they are assigned to a robot
@@ -135,7 +135,7 @@ void Play::distributeRoles() noexcept {
         flagMap[roles[currentMaxRobots]->getName()].priority = DealerFlagPriority::CARD;
         flagMap[roles[currentMaxRobots]->getName()].forcedID = cardId;
         stpInfos[roles[currentMaxRobots]->getName()].setShouldAvoidBall(true);
-        stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0, -field.playArea.width() / 2));
+        stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0.0, -field.playArea.width() / 2));
     }
     // Only keep the first n roles, where n is the amount of robots we have
     // This order is based on the order of the roles array
@@ -144,7 +144,6 @@ void Play::distributeRoles() noexcept {
     }
     auto distribution = dealer.distribute(world->getWorld()->getUs(), flagMap, stpInfos);
 
-    // TODO-Max if role exists in oldStpInfos then copy those.
     // Clear the stpInfos for the new role assignment
     bool cardIdAssigned = false;
     for (auto &role : roles) {

roboteam_ai/src/stp/PlayEvaluator.cpp

@@ -5,6 +5,7 @@
 #include "stp/PlayEvaluator.h"
 
 #include <stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.h>
+#include <stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h>
 #include <stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.h>
 #include <stp/evaluations/game_states/FreeKickThemGameStateEvaluation.h>
 #include <stp/evaluations/game_states/FreeKickUsGameStateEvaluation.h>
@@ -44,6 +45,8 @@ uint8_t PlayEvaluator::updateGlobalEvaluation(GlobalEvaluation& evaluation, cons
             return evaluation::BallPlacementThemGameStateEvaluation().metricCheck(world, &field);
         case GlobalEvaluation::BallPlacementUsGameState:
             return evaluation::BallPlacementUsGameStateEvaluation().metricCheck(world, &field);
+        case GlobalEvaluation::BallPlacementUsDirectGameState:
+            return evaluation::BallPlacementUsDirectGameStateEvaluation().metricCheck(world, &field);
         case GlobalEvaluation::FreeKickThemGameState:
             return evaluation::FreeKickThemGameStateEvaluation().metricCheck(world, &field);
         case GlobalEvaluation::FreeKickUsGameState: