From e461f691a0c4bd2b039bd7b5eda8125851c06ec4 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 10:26:22 +0100 Subject: [PATCH 1/8] Remove build warnings from ai code --- .../roboteam_ai/control/ControlModule.h | 6 +-- .../roboteam_ai/control/ControlUtils.h | 3 +- .../BBTrajectories/WorldObjects.h | 8 +--- .../control/positionControl/PositionControl.h | 3 +- .../roboteam_ai/interface/api/Output.h | 1 - roboteam_ai/include/roboteam_ai/stp/Skill.h | 3 +- .../stp/computations/PositionComputations.h | 4 +- .../global/BallOnOurSideGlobalEvaluation.h | 3 +- .../global/BallOnTheirSideGlobalEvaluation.h | 3 +- .../TheyDoNotHaveBallGlobalEvaluation.h | 3 +- .../global/TheyHaveBallGlobalEvaluation.h | 3 +- .../global/WeDoNotHaveBallGlobalEvaluation.h | 3 +- .../global/WeHaveBallGlobalEvaluation.h | 3 +- .../roboteam_ai/stp/tactics/KeeperBlockBall.h | 3 +- .../include/roboteam_ai/utilities/Dealer.h | 3 +- .../roboteam_ai/world/FieldComputations.h | 7 +--- roboteam_ai/src/control/ControlModule.cpp | 12 ++---- roboteam_ai/src/control/ControlUtils.cpp | 2 +- .../BBTrajectories/WorldObjects.cpp | 30 +++++++------- .../positionControl/PositionControl.cpp | 10 +++-- .../pathTracking/PidTracking.cpp | 2 +- roboteam_ai/src/interface/api/Output.cpp | 2 - roboteam_ai/src/stp/Play.cpp | 2 +- roboteam_ai/src/stp/PlayEvaluator.cpp | 3 +- roboteam_ai/src/stp/Skill.cpp | 4 +- .../stp/computations/PositionComputations.cpp | 39 ++++++++----------- .../game_states/HaltGameStateEvaluation.cpp | 2 +- .../NormalPlayGameStateEvaluation.cpp | 2 +- .../game_states/StopGameStateEvaluation.cpp | 2 +- .../global/BallOnOurSideGlobalEvaluation.cpp | 2 +- .../BallOnTheirSideGlobalEvaluation.cpp | 2 +- .../TheyDoNotHaveBallGlobalEvaluation.cpp | 2 +- .../global/TheyHaveBallGlobalEvaluation.cpp | 2 +- .../WeDoNotHaveBallGlobalEvaluation.cpp | 2 +- .../global/WeHaveBallGlobalEvaluation.cpp | 2 +- .../src/stp/plays/defensive/DefendPass.cpp | 2 +- .../src/stp/plays/defensive/DefendShot.cpp | 2 +- .../stp/plays/defensive/KeeperKickBall.cpp | 2 +- .../src/stp/plays/offensive/Attack.cpp | 2 +- .../src/stp/plays/offensive/AttackingPass.cpp | 2 +- .../src/stp/plays/offensive/ChippingPass.cpp | 4 +- .../AggressiveStopFormation.cpp | 2 +- .../referee_specific/BallPlacementThem.cpp | 2 +- .../referee_specific/BallPlacementUs.cpp | 2 +- .../DefensiveStopFormation.cpp | 2 +- .../plays/referee_specific/FreeKickThem.cpp | 2 +- .../plays/referee_specific/FreeKickUsPass.cpp | 2 +- .../src/stp/plays/referee_specific/Halt.cpp | 2 +- .../plays/referee_specific/KickOffThem.cpp | 2 +- .../referee_specific/KickOffThemPrepare.cpp | 2 +- .../stp/plays/referee_specific/KickOffUs.cpp | 2 +- .../referee_specific/KickOffUsPrepare.cpp | 2 +- .../plays/referee_specific/PenaltyThem.cpp | 2 +- .../referee_specific/PenaltyThemPrepare.cpp | 2 +- .../stp/plays/referee_specific/PenaltyUs.cpp | 2 +- .../referee_specific/PenaltyUsPrepare.cpp | 2 +- roboteam_ai/src/stp/skills/Chip.cpp | 2 +- roboteam_ai/src/stp/skills/GoToPos.cpp | 2 +- roboteam_ai/src/stp/skills/Kick.cpp | 2 +- roboteam_ai/src/stp/skills/Orbit.cpp | 2 +- roboteam_ai/src/stp/skills/OrbitAngular.cpp | 2 +- roboteam_ai/src/stp/skills/Rotate.cpp | 2 +- roboteam_ai/src/stp/skills/TestSkill.cpp | 2 +- .../src/stp/tactics/KeeperBlockBall.cpp | 2 +- roboteam_ai/src/stp/tactics/TestTactic.cpp | 4 +- .../src/stp/tactics/active/ChipAtPos.cpp | 2 +- .../src/stp/tactics/active/GetBall.cpp | 2 +- .../src/stp/tactics/active/KickAtPos.cpp | 2 +- .../src/stp/tactics/active/OrbitKick.cpp | 2 +- .../src/stp/tactics/passive/BallStandBack.cpp | 2 +- .../src/stp/tactics/passive/BlockBall.cpp | 4 +- .../src/stp/tactics/passive/Formation.cpp | 2 +- roboteam_ai/src/stp/tactics/passive/Halt.cpp | 4 +- roboteam_ai/src/utilities/Dealer.cpp | 9 +---- roboteam_ai/src/utilities/IOManager.cpp | 4 +- roboteam_ai/src/world/FieldComputations.cpp | 4 +- roboteam_ai/test/StpTests/TacticTests.cpp | 4 +- 77 files changed, 127 insertions(+), 161 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/ControlModule.h b/roboteam_ai/include/roboteam_ai/control/ControlModule.h index 1d74ec173..1b56a9052 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlModule.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlModule.h @@ -35,9 +35,8 @@ class ControlModule { /** * @brief Limits the velocity with a control_constants value * @param command Robot command that needs to be checked - * @param robot Info about the robot */ - static void limitVel(rtt::RobotCommand& command, std::optional robot); + static void limitVel(rtt::RobotCommand& command); /** * @brief Limits the angular velocity with a control_constants value @@ -57,9 +56,8 @@ class ControlModule { * @brief Limits the current robot command and adds it to the list of commands to be sent * @param command Robot command that needs to be added * @param robot Info about the robot - * @param data Info about world */ - static void addRobotCommand(std::optional robot, const rtt::RobotCommand& command, const rtt::world::World* data) noexcept; + static void addRobotCommand(std::optional robot, const rtt::RobotCommand& command) noexcept; /** * @brief Sends all commands to robothub */ diff --git a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h index 03a93438d..fd407a6d6 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h @@ -82,10 +82,9 @@ class ControlUtils { /** * @brief Determines the chip force based on the distance and the type of chip * @param distance distance to the target - * @param shotType type of the chip * @return a chip speed between min and max chip speed */ - static double determineChipForce(const double distance, stp::ShotType shotType) noexcept; + static double determineChipForce(const double distance) noexcept; /** * @brief Determine the max allowed velocity considering the game state and whether the robot has the ball diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h index 9cb845a67..5cf2d2180 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h @@ -75,12 +75,9 @@ class WorldObjects { * including the part that might already be completed. Only call this function for a new trajectory * @param field used for checking collisions with the field * @param BBTrajectory the trajectory to check for collisions - * @param computedPaths the paths of our robots - * @param robotId Id of the robot * @return optional with rtt::BB::CollisionData */ - std::optional getFirstDefenseAreaCollision(const rtt::Field &field, const rtt::Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId); + std::optional getFirstDefenseAreaCollision(const rtt::Field &field, const rtt::Trajectory2D &Trajectory); /** * @brief Takes a calculated path of a robot and checks points along the path if they are outside the @@ -100,11 +97,10 @@ class WorldObjects { * @param field Used for information about the field * @param collisionDatas std::vector which rtt::BB::CollisionData can be added to * @param pathPoints std::vector with path points - * @param robotId ID of the robot * @param timeStep Time between pathpoints * @param completedTimeSteps Number of completed time steps */ - void calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, double timeStep, + void calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, size_t completedTimeSteps); /** diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index 6c506e457..b2c9db3c6 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -110,14 +110,13 @@ class PositionControl { * distance to the first collision on the path * @param world a pointer to the current world * @param field the field object, used onwards by the collision detector - * @param robotId the ID of the robot for which the path is calculated * @param firstCollision location of the first collision on the current path * @param trajectoryAroundCollision the trajectory to the intermediate point * @param avoidObjects whether or not to avoid objects * @param startTime the time at which the trajectory starts * @return A score for the trajectory */ - double calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, + double calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional &firstCollision, Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime = 0); /** diff --git a/roboteam_ai/include/roboteam_ai/interface/api/Output.h b/roboteam_ai/include/roboteam_ai/interface/api/Output.h index 164f8c970..130419d71 100644 --- a/roboteam_ai/include/roboteam_ai/interface/api/Output.h +++ b/roboteam_ai/include/roboteam_ai/interface/api/Output.h @@ -54,7 +54,6 @@ class Output { static void setRuleSetName(std::string name); static void setKeeperId(int id); - static void setBallPlacerId(int id); }; } // namespace rtt::ai::interface diff --git a/roboteam_ai/include/roboteam_ai/stp/Skill.h b/roboteam_ai/include/roboteam_ai/stp/Skill.h index 93c16f426..028299e05 100644 --- a/roboteam_ai/include/roboteam_ai/stp/Skill.h +++ b/roboteam_ai/include/roboteam_ai/stp/Skill.h @@ -25,9 +25,8 @@ class Skill { /** * @brief Forwards the current robot command to the ControlModule and refreshes it - * @param data const pointer to world */ - virtual void forwardRobotCommand(world::World const* data) noexcept; + virtual void forwardRobotCommand() noexcept; /** * @brief Resets the internal robot command diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index 12ad4e11b..856f3f512 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -149,13 +149,11 @@ class PositionComputations { /** * @brief Recalculates info for the position of our robots to not interfere with passing * @param stpInfos The current stpInfos - * @param roles The current roles * @param field The current field * @param world The current world * @param passInfo The current passInfo */ - static void recalculateInfoForNonPassers(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, - const Field &field, world::World *world, Vector2 passLocation) noexcept; + static void recalculateInfoForNonPassers(std::unordered_map &stpInfos, const Field &field, world::World *world, Vector2 passLocation) noexcept; private: /** diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnOurSideGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnOurSideGlobalEvaluation.h index e8b6576d0..94e92da07 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnOurSideGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnOurSideGlobalEvaluation.h @@ -16,10 +16,9 @@ class BallOnOurSideGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for ball on our side * @param world The current world - * @param field The current field * @return The score of ball on our side */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.h index aeaebae55..0ebc16a9d 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.h @@ -16,10 +16,9 @@ class BallOnTheirSideGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for ball on their side * @param world The current world - * @param field The current field * @return The score of ball on their side */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.h index 0c65ea422..f9067db7c 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.h @@ -16,10 +16,9 @@ class TheyDoNotHaveBallGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for they do not have ball * @param world The current world - * @param field The current field * @return The score of they do not have ball */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyHaveBallGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyHaveBallGlobalEvaluation.h index 5e356bba4..1d86f703a 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyHaveBallGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/TheyHaveBallGlobalEvaluation.h @@ -16,10 +16,9 @@ class TheyHaveBallGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for they have ball * @param world The current world - * @param field The current field * @return The score of they have ball */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.h index 38f5bb6e0..3eb0a88fe 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.h @@ -16,10 +16,9 @@ class WeDoNotHaveBallGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for we don't have ball * @param world The current world - * @param field The current field * @return The score of we don't have ball */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeHaveBallGlobalEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeHaveBallGlobalEvaluation.h index d21e7d586..15f2c2813 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeHaveBallGlobalEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/global/WeHaveBallGlobalEvaluation.h @@ -16,10 +16,9 @@ class WeHaveBallGlobalEvaluation : public BaseEvaluation { /** * @brief Calculates the score for we have ball * @param world The current world - * @param field The current field * @return The score of we have ball */ - [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field*) const noexcept override; /** * @brief Retrieves the name of the evaluation diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h index 28c5ebb5d..33ade1860 100644 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h +++ b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h @@ -31,10 +31,9 @@ class KeeperBlockBall : public Tactic { /** * @brief Is this tactic failing during execution (go back to the previous tactic) * This tactic can never fail, so always returns false - * @param info StpInfo can be used to check some data * @return true, tactic will fail (go back to prev tactic), false execution will continue as usual */ - bool isTacticFailing(const StpInfo &info) noexcept override; + bool isTacticFailing(const StpInfo &) noexcept override; /** * @brief Should this tactic be reset (go back to the first skill of this tactic) diff --git a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h index da7ae8a87..7004d6f29 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h @@ -143,11 +143,10 @@ class Dealer { /** * @brief Calculates the cost of travelling a certain distance * @param distance The distance to travel - * @param fieldWidth The width of the field * @param fieldHeight The height of the field * @return Cost of travelling that distance */ - static double costForDistance(double distance, double fieldWidth, double fieldHeight); + static double costForDistance(double distance, double fieldHeight); /** * @brief Calculates the cost of a property of a robot diff --git a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h index df78f8856..9dfdceedf 100644 --- a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h +++ b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h @@ -78,11 +78,9 @@ class FieldComputations { * @param field The field class which is used to determine the boundaries of the field. * @param point The point to be projected to a valid position * @param avoidObjects Struct indicating which areas of the field should be avoided. Defaults to avoid entering the defense area and leaving the field - * @param fieldMargin The outwards margin in which the field area will get expanded/shrunk in all directions. A positive value means that the field area will be - * expanded, a negative value means that the field area will be shrunk. * @return The position closest to the given point that fulfills the criteria set in avoidObjects (except ball avoidance) */ - static Vector2 projectPointToValidPosition(const rtt::Field &field, Vector2 point, stp::AvoidObjects avoidObjects = {}, double fieldMargin = 0.0); + static Vector2 projectPointToValidPosition(const rtt::Field &field, Vector2 point, stp::AvoidObjects avoidObjects = {}); /** * @brief Projects the given point into the field on a line between two given points. @@ -103,13 +101,12 @@ class FieldComputations { * @param point The point to be projected to a valid position * @param p1 First point on the line * @param p2 Second point on the line - * @param avoidObjects Struct indicating which areas of the field should be avoided. Defaults to avoid entering the defense area and leaving the field * @param fieldMargin The outwards margin in which the field area will get expanded/shrunk in all directions. A positive value means that the field area will be * expanded, a negative value means that the field area will be shrunk. * @return The closest valid position to the given point on the line between p1 and p2. * If no such position can be found, return the closest valid position to the given point (but not on the given line). */ - static Vector2 projectPointToValidPositionOnLine(const Field &field, Vector2 point, Vector2 p1, Vector2 p2, stp::AvoidObjects avoidObjects = {}, double fieldMargin = 0); + static Vector2 projectPointToValidPositionOnLine(const Field &field, Vector2 point, Vector2 p1, Vector2 p2, double fieldMargin = 0); /** * @brief Get the percentage of goal visible from a given point, i.e. how much of the goal can be reached by directly shooting a ball over the ground from a given point without diff --git a/roboteam_ai/src/control/ControlModule.cpp b/roboteam_ai/src/control/ControlModule.cpp index f7ff66f68..de30fcad9 100644 --- a/roboteam_ai/src/control/ControlModule.cpp +++ b/roboteam_ai/src/control/ControlModule.cpp @@ -20,12 +20,13 @@ void ControlModule::rotateRobotCommand(rtt::RobotCommand& command) { } void ControlModule::limitRobotCommand(rtt::RobotCommand& command, std::optional robot) { - limitVel(command, robot); + limitVel(command); limitAngularVel(command, robot); } -void ControlModule::limitVel(rtt::RobotCommand& command, std::optional 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())); } @@ -53,13 +54,8 @@ void ControlModule::limitAngularVel(rtt::RobotCommand& command, std::optional robot, const rtt::RobotCommand& command, const rtt::world::World* data) noexcept { +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 - - if (robot && robot->get()) { - // TODO: Fix this visualisation - Angle target = command.targetAngle; - } // If we are not left, commands should be rotated (because we play as right) if (!GameSettings::isLeft()) { rotateRobotCommand(robot_command); diff --git a/roboteam_ai/src/control/ControlUtils.cpp b/roboteam_ai/src/control/ControlUtils.cpp index 76472e061..a2a981af8 100644 --- a/roboteam_ai/src/control/ControlUtils.cpp +++ b/roboteam_ai/src/control/ControlUtils.cpp @@ -101,7 +101,7 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotType sho return std::clamp(velocity, stp::control_constants::MIN_KICK_POWER, stp::control_constants::MAX_KICK_POWER); } /// Calculates the chip force -double ControlUtils::determineChipForce(const double distance, stp::ShotType shotType) noexcept { +double ControlUtils::determineChipForce(const double distance) noexcept { // Factor that determines the chipping force double chipFactor = 3.0; // Calculate the velocity based on this function with the previously set limitingFactor diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index dcea37d42..64aeb4e36 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -25,7 +25,7 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W calculateFieldCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidDefenseArea) { - calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, completedTimeSteps[robotId]); + calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidBall) { calculateBallCollisions(world, collisionDatas, pathPoints, timeStep, avoidObjects.avoidBallDist, completedTimeSteps[robotId], startTime); @@ -46,13 +46,12 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W } } -std::optional WorldObjects::getFirstDefenseAreaCollision(const rtt::Field &field, const Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId) { +std::optional WorldObjects::getFirstDefenseAreaCollision(const rtt::Field &field, const Trajectory2D &Trajectory) { double timeStep = 0.1; auto pathPoints = Trajectory.getPathApproach(timeStep); std::vector collisionDatas; - calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep, 0); + calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, 0); if (!collisionDatas.empty()) { return collisionDatas[0]; @@ -74,8 +73,8 @@ void WorldObjects::calculateFieldCollisions(const rtt::Field &field, std::vector } } -void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, - double timeStep, size_t completedTimeSteps) { +void WorldObjects::calculateDefenseAreaCollisions(const rtt::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, double timeStep, + size_t completedTimeSteps) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = rtt::ai::FieldComputations::getDefenseAreaMargin(); const auto &ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); const auto &theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1); @@ -97,8 +96,9 @@ void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std:: const Vector2 &VelocityBall = world->getWorld()->getBall()->get()->velocity; const Vector2 &startPositionBall = world->getWorld()->getBall()->get()->position + VelocityBall * startTime; - - for (double loopTime = 0; loopTime < ballAvoidanceTime; loopTime += timeStep, completedTimeSteps++) { + int numSteps = static_cast(ballAvoidanceTime / timeStep); + for (int i = 0; i <= numSteps; ++i, completedTimeSteps++) { + double loopTime = i * timeStep; if (completedTimeSteps + 1 >= pathPoints.size()) { return; } @@ -141,8 +141,9 @@ void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, const std::vector &theirRobots = world->getWorld()->getThem(); const double maxCollisionCheckTime = 0.7 - startTime; const double avoidanceDistance = 4 * ai::Constants::ROBOT_RADIUS_MAX(); - - for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps++) { + int numSteps = static_cast(maxCollisionCheckTime / timeStep); + for (int i = 0; i <= numSteps; ++i, completedTimeSteps++) { + double loopTime = i * timeStep; if (completedTimeSteps + 1 >= pathPoints.size()) { return; } @@ -195,8 +196,10 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s const double maxCollisionCheckTime = 0.7 - startTime; const double avoidanceDistance = 2 * ai::Constants::ROBOT_RADIUS() + 2 * ai::stp::control_constants::GO_TO_POS_ERROR_MARGIN; - for (double loopTime = 0; loopTime < maxCollisionCheckTime; loopTime += timeStep, completedTimeSteps[robotId]++) { - if (completedTimeSteps[robotId] + 1 >= pathPoints.size()) { + int numSteps = static_cast(maxCollisionCheckTime / timeStep); + for (int i = 0; i <= numSteps; ++i, completedTimeSteps[robotId]++) { + double loopTime = i * timeStep; + if (static_cast(completedTimeSteps[robotId] + 1) >= pathPoints.size()) { return; } const double startPointX_OurRobot = pathPoints[completedTimeSteps[robotId]].x; @@ -220,7 +223,7 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s double startPointX_OtherRobot, startPointY_OtherRobot, velocityX_OtherRobot, velocityY_OtherRobot; - if (completedTimeStepsIt != completedTimeSteps.end() && computedPathsIt != computedPaths.end() && completedTimeStepsIt->second + 1 < computedPathsIt->second.size()) { + if (completedTimeStepsIt != completedTimeSteps.end() && computedPathsIt != computedPaths.end() && static_cast(completedTimeStepsIt->second + 1) < computedPathsIt->second.size()) { startPointX_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].x; startPointY_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].y; velocityX_OtherRobot = (computedPathsIt->second[completedTimeStepsIt->second + 1].x - startPointX_OtherRobot) / timeStep; @@ -265,7 +268,6 @@ void WorldObjects::calculateBallPlacementCollision(const rtt::world::World *worl auto ballPlacementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); auto startPositionBall = world->getWorld()->getBall()->get()->position; LineSegment ballPlacementLine(startPositionBall, ballPlacementPos); - double time = completedTimeSteps * timeStep; for (size_t i = completedTimeSteps; i < pathPoints.size(); i++) { if (ballPlacementLine.distanceToLine(pathPoints[i]) < ai::stp::control_constants::AVOID_BALL_DISTANCE) { diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index be3a3109c..0bcacb142 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -116,7 +116,7 @@ rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt:: return commandCollision; } -double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional &firstCollision, +double PositionControl::calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional &firstCollision, Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime) { double totalTime = trajectoryAroundCollision.getTotalTime(); double score = totalTime + startTime; @@ -128,7 +128,7 @@ double PositionControl::calculateScore(const rtt::world::World *world, const rtt score += std::max(0.0, 3 - firstCollision.value().collisionTime - startTime); if (avoidObjects.shouldAvoidDefenseArea) { - auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision, computedPaths, robotId); + auto defenseAreaCollision = worldObjects.getFirstDefenseAreaCollision(field, trajectoryAroundCollision); if (defenseAreaCollision.has_value()) { score += std::max(0.0, 1 - defenseAreaCollision.value().collisionTime - startTime) * 10; score += 5; @@ -172,7 +172,9 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects, completedTimeSteps); double maxLoopTime = intermediatePathCollision.has_value() ? intermediatePathCollision.value().collisionTime : trajectoryToIntermediatePoint.getTotalTime(); - for (double loopTime = 0; loopTime < maxLoopTime; loopTime += timeStep) { + int numSteps = static_cast(maxLoopTime / timeStep); + for (int i = 0; i <= numSteps; ++i) { + double loopTime = i * timeStep; Vector2 newStartPosition = trajectoryToIntermediatePoint.getPosition(loopTime); Vector2 newStartVelocity = trajectoryToIntermediatePoint.getVelocity(loopTime); Trajectory2D trajectoryAroundCollision(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); @@ -181,7 +183,7 @@ std::optional PositionControl::findNewTrajectory(const rtt::world: trajectoryToIntermediatePoint.addTrajectory(trajectoryAroundCollision, loopTime); return trajectoryToIntermediatePoint; } else { - double score = calculateScore(world, field, robotId, firstCollision, trajectoryAroundCollision, avoidObjects, loopTime); + double score = calculateScore(world, field, firstCollision, trajectoryAroundCollision, avoidObjects, loopTime); if (score < bestScore) { bestScore = score; bestTrajectory = trajectoryToIntermediatePoint; diff --git a/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp b/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp index 4a2cd33f7..32c3e8aed 100644 --- a/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp +++ b/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp @@ -2,7 +2,7 @@ namespace rtt::ai::control { -Position PidTracking::trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) { +Position PidTracking::trackPath(const Vector2 ¤tPosition, const Vector2&, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) { PositionControlUtils::removeFirstIfReached(pathPoints, currentPosition); if (pidMapping.find(robotId) == pidMapping.end()) { pidMapping[robotId] = std::make_pair(PID(), PID()); diff --git a/roboteam_ai/src/interface/api/Output.cpp b/roboteam_ai/src/interface/api/Output.cpp index 3385a9500..d8af0805d 100644 --- a/roboteam_ai/src/interface/api/Output.cpp +++ b/roboteam_ai/src/interface/api/Output.cpp @@ -41,8 +41,6 @@ void Output::setRuleSetName(std::string name) { Output::interfaceGameState.ruleS void Output::setKeeperId(int id) { Output::interfaceGameState.keeperId = id; } -void Output::setBallPlacerId(int id) {} - const GameState &Output::getInterfaceGameState() { return Output::interfaceGameState; } void Output::setInterfaceGameState(GameState interfaceGameState) { diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 9fe2eb184..b02649fd2 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -139,7 +139,7 @@ void Play::distributeRoles() noexcept { } // 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 - for (int i = sizeUs; i < roles.size(); i++) { + for (size_t i = sizeUs; i < roles.size(); i++) { flagMap.erase(roles[i]->getName()); } auto distribution = dealer.distribute(world->getWorld()->getUs(), flagMap, stpInfos); diff --git a/roboteam_ai/src/stp/PlayEvaluator.cpp b/roboteam_ai/src/stp/PlayEvaluator.cpp index c290ccdb5..5b7a03458 100644 --- a/roboteam_ai/src/stp/PlayEvaluator.cpp +++ b/roboteam_ai/src/stp/PlayEvaluator.cpp @@ -104,7 +104,8 @@ bool PlayEvaluator::checkEvaluation(GlobalEvaluation globalEvaluation, const rtt } uint8_t PlayEvaluator::calculateScore(std::vector& scoring) { - double scoreTotal, weightTotal = 0; + double scoreTotal = 0; + double weightTotal = 0; for (auto& factor : scoring) { scoreTotal += factor.evaluationScore; weightTotal += factor.weight; diff --git a/roboteam_ai/src/stp/Skill.cpp b/roboteam_ai/src/stp/Skill.cpp index 8f8dcddfe..832343020 100644 --- a/roboteam_ai/src/stp/Skill.cpp +++ b/roboteam_ai/src/stp/Skill.cpp @@ -9,9 +9,9 @@ namespace rtt::ai::stp { -void Skill::forwardRobotCommand(world::World const* data) noexcept { +void Skill::forwardRobotCommand() noexcept { // The command gets passed to the ControlModule here, for checking and limiting - control::ControlModule::addRobotCommand(robot, command, data); + control::ControlModule::addRobotCommand(robot, command); // refresh the robot command after it has been sent refreshRobotCommand(); diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 0e0ca66d9..7fc04843c 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -264,7 +264,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma auto defenderNames = std::vector{}; auto wallerNames = std::vector{}; auto additionalWallerNames = std::vector{}; - for (int i = 0; i < world->getWorld()->getUs().size(); i++) { + for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { if (roles[i]->getName().find("waller") != std::string::npos) { wallerNames.emplace_back(roles[i]->getName()); } else if (roles[i]->getName().find("defender") != std::string::npos) { @@ -294,19 +294,17 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma ComputationManager::calculatedEnemyMapIds.clear(); // If defenders do not have a position yet, don't do hungarian algorithm if (activeDefenderNames.empty()) { - auto loopSize = std::min(defenderNames.size(), enemyMap.size()); - for (int i = 0; i < loopSize; i++) { + size_t loopSize = std::min(defenderNames.size(), enemyMap.size()); + for (size_t i = 0; i < loopSize; i++) { Vector2 defendPostion = enemyMap.begin()->second.position; - Vector2 defendSpeed = enemyMap.begin()->second.velocity; if (mustStayOnOurSide && defendPostion.x > 0) { defendPostion.x = 0; - defendSpeed.x = 0; } stpInfos["defender_" + std::to_string(i)].setPositionToDefend(defendPostion); ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); enemyMap.erase(enemyMap.begin()); } - for (int i = loopSize; i < defenderNames.size(); i++) { + for (size_t i = loopSize; i < defenderNames.size(); i++) { additionalWallerNames.emplace_back("defender_" + std::to_string(i)); } } else { @@ -314,7 +312,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma std::vector> cost_matrix; cost_matrix.resize(activeDefenderNames.size()); int row_length = std::min(activeDefenderNames.size(), enemyMap.size()); - for (int i = 0; i < activeDefenderNames.size(); i++) { + for (size_t i = 0; i < activeDefenderNames.size(); i++) { cost_matrix[i].resize(row_length); // Check if there are still enemies left if (enemyMap.empty()) continue; @@ -330,8 +328,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma // active pass defender std::vector assignments; rtt::Hungarian::Solve(cost_matrix, assignments); - int currentWallerIndex = 0; - for (int i = 0; i < activeDefenderNames.size(); i++) { + for (size_t i = 0; i < activeDefenderNames.size(); i++) { // If assignments is -1, it means the pass defender does not get an enemy assigned to it, because there are more pass defenders than enemies if (assignments[i] == -1) { additionalWallerNames.emplace_back(activeDefenderNames[i]); @@ -341,7 +338,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } } } - for (int i = 0; i < wallerNames.size() + additionalWallerNames.size(); ++i) { + for (size_t i = 0; i < wallerNames.size() + additionalWallerNames.size(); ++i) { // For each waller, stand in the right wall position and look at the ball auto positionToMoveTo = PositionComputations::getWallPosition(i, wallerNames.size() + additionalWallerNames.size(), field, world); std::string currentWallerName; @@ -370,7 +367,7 @@ void PositionComputations::calculateInfoForAttackers(std::unordered_map{}; - for (int i = 0; i < world->getWorld()->getUs().size(); i++) { + for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { if (roles[i]->getName().find("attacker") != std::string::npos) { attackerNames.emplace_back(roles[i]->getName()); } @@ -405,7 +402,7 @@ void PositionComputations::calculateInfoForFormation(std::unordered_map{}; auto formationMidNames = std::vector{}; auto formationFrontNames = std::vector{}; - for (int i = 0; i < world->getWorld()->getUs().size(); i++) { + for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { if (roles[i]->getName().find("formation_back") != std::string::npos) { formationBackNames.emplace_back(roles[i]->getName()); } else if (roles[i]->getName().find("formation_mid") != std::string::npos) { @@ -418,13 +415,13 @@ void PositionComputations::calculateInfoForFormation(std::unordered_map{}; auto formationMidNames = std::vector{}; auto formationFrontNames = std::vector{}; - for (int i = 0; i < world->getWorld()->getUs().size(); i++) { + for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { if (roles[i]->getName().find("formation_back") != std::string::npos) { formationBackNames.emplace_back(roles[i]->getName()); } else if (roles[i]->getName().find("formation_mid") != std::string::npos) { @@ -448,26 +445,24 @@ void PositionComputations::calculateInfoForFormationOurSide(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world, +void PositionComputations::recalculateInfoForNonPassers(std::unordered_map &stpInfos, const Field &field, world::World *world, Vector2 passLocation) noexcept { auto ballPosition = world->getWorld()->getBall()->get()->position; // Make a list of all robots that are not the passer, receiver or keeper, which need to make sure they are not in the way of the pass auto toBeCheckedRobots = std::vector{}; for (auto &role : stpInfos) { if (role.second.getRobot().has_value()) { - auto robotId = role.second.getRobot()->get()->getId(); auto robotName = role.first; if (robotName != "keeper" && robotName != "passer" && robotName != "receiver" && robotName != "striker" && robotName != "free_kick_taker") { toBeCheckedRobots.emplace_back(role.first); diff --git a/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp index 48b551b68..9f3af34a9 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp @@ -7,7 +7,7 @@ #include "utilities/GameStateManager.hpp" namespace rtt::ai::stp::evaluation { -uint8_t HaltGameStateEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t HaltGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { return GameStateManager::getCurrentGameState().getStrategyName() == "halt" ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation diff --git a/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp index 52d94d83d..7b1e7dbf4 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp @@ -8,7 +8,7 @@ namespace rtt::ai::stp::evaluation { -uint8_t NormalPlayGameStateEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t NormalPlayGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { return GameStateManager::getCurrentGameState().getStrategyName() == "normal_play" ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp index 388785d99..a977c6648 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp @@ -8,7 +8,7 @@ namespace rtt::ai::stp::evaluation { -uint8_t StopGameStateEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t StopGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { return GameStateManager::getCurrentGameState().getStrategyName() == "stop" ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp index 634cb8070..a0a93302a 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp @@ -6,7 +6,7 @@ #include "stp/evaluations/global/BallOnOurSideGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t BallOnOurSideGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t BallOnOurSideGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { return world->getWorld()->getBall().value()->position.x < 0 ? control_constants::FUZZY_TRUE : control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.cpp index 8b87b8b20..64be54dca 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallOnTheirSideGlobalEvaluation.cpp @@ -6,7 +6,7 @@ #include "stp/evaluations/global/BallOnTheirSideGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t BallOnTheirSideGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t BallOnTheirSideGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { return world->getWorld()->getBall().value()->position.x >= 0 ? control_constants::FUZZY_TRUE : control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.cpp index 3661d5939..4b5b0c7a8 100644 --- a/roboteam_ai/src/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.cpp @@ -6,7 +6,7 @@ #include "stp/evaluations/global/TheyDoNotHaveBallGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t TheyDoNotHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t TheyDoNotHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { auto& them = world->getWorld()->getThem(); // If there are no bots, they don't have ball diff --git a/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp index ba766178f..4189dc101 100644 --- a/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp @@ -6,7 +6,7 @@ #include "stp/evaluations/global/TheyHaveBallGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t TheyHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t TheyHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { auto& them = world->getWorld()->getThem(); // If there are no bots, they don't have ball diff --git a/roboteam_ai/src/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.cpp index aa1f63e1e..5b6841905 100644 --- a/roboteam_ai/src/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.cpp @@ -5,7 +5,7 @@ #include "stp/evaluations/global/WeDoNotHaveBallGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t WeDoNotHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t WeDoNotHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { auto& us = world->getWorld()->getUs(); // If there are no bots, we don't have ball diff --git a/roboteam_ai/src/stp/evaluations/global/WeHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/WeHaveBallGlobalEvaluation.cpp index c943e58a3..e17659442 100644 --- a/roboteam_ai/src/stp/evaluations/global/WeHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/WeHaveBallGlobalEvaluation.cpp @@ -6,7 +6,7 @@ #include "stp/evaluations/global/WeHaveBallGlobalEvaluation.h" namespace rtt::ai::stp::evaluation { -uint8_t WeHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { +uint8_t WeHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { auto& us = world->getWorld()->getUs(); // If there are no bots, we don't have ball diff --git a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp index 03816680d..080b45bcb 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp @@ -47,7 +47,7 @@ DefendPass::DefendPass() : Play() { }; } -uint8_t DefendPass::score(const rtt::Field& field) noexcept { +uint8_t DefendPass::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp index 521203e70..b53c47db5 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp @@ -44,7 +44,7 @@ DefendShot::DefendShot() : Play() { }; } -uint8_t DefendShot::score(const rtt::Field& field) noexcept { +uint8_t DefendShot::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp index 21d74b257..fb33e7d1e 100644 --- a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp +++ b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp @@ -72,7 +72,7 @@ Dealer::FlagMap KeeperKickBall::decideRoleFlags() const noexcept { void KeeperKickBall::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); - PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo.passLocation); + PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); stpInfos["keeper"].setShouldAvoidTheirRobots(false); stpInfos["keeper"].setShouldAvoidOurRobots(false); diff --git a/roboteam_ai/src/stp/plays/offensive/Attack.cpp b/roboteam_ai/src/stp/plays/offensive/Attack.cpp index 155fe44f7..e20851462 100644 --- a/roboteam_ai/src/stp/plays/offensive/Attack.cpp +++ b/roboteam_ai/src/stp/plays/offensive/Attack.cpp @@ -83,7 +83,7 @@ void Attack::calculateInfoForRoles() noexcept { stpInfos["striker"].setPositionToShootAt(goalTarget); stpInfos["striker"].setKickOrChip(KickOrChip::KICK); stpInfos["striker"].setShotType(ShotType::MAX); - PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, goalTarget); + PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, goalTarget); } bool Attack::shouldEndPlay() noexcept { diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 4a50505e9..01b6a9559 100644 --- a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp @@ -79,7 +79,7 @@ void AttackingPass::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); - PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo.passLocation); + PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); if (!ballKicked()) { stpInfos["passer"].setPositionToShootAt(passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp index e08bf657c..1b0eaec7d 100644 --- a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp @@ -49,7 +49,7 @@ ChippingPass::ChippingPass() : Play() { }; } -uint8_t ChippingPass::score(const rtt::Field& field) noexcept { +uint8_t ChippingPass::score(const rtt::Field&) noexcept { // Robots can't chip yet return 0; // passInfo = stp::computations::PassComputations::calculatePass(gen::ChippingPass, world, field); @@ -81,7 +81,7 @@ void ChippingPass::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); - PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo.passLocation); + PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); if (!ballKicked()) { stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp index dafa4dec8..e0b13b129 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp @@ -35,7 +35,7 @@ AggressiveStopFormation::AggressiveStopFormation() : Play() { }; } -uint8_t AggressiveStopFormation::score(const rtt::Field& field) noexcept { +uint8_t AggressiveStopFormation::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index de6765dcd..9dd5dad26 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -37,7 +37,7 @@ BallPlacementThem::BallPlacementThem() : Play() { }; } -uint8_t BallPlacementThem::score(const rtt::Field& field) noexcept { +uint8_t BallPlacementThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp index 49e86e8f4..ce1999dd6 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp @@ -29,7 +29,7 @@ BallPlacementUs::BallPlacementUs() : Play() { std::make_unique("formation_mid_2"), std::make_unique("formation_front_2")}; } -uint8_t BallPlacementUs::score(const rtt::Field& field) noexcept { +uint8_t BallPlacementUs::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp index fce2f967c..4ae9e1c9b 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp @@ -36,7 +36,7 @@ DefensiveStopFormation::DefensiveStopFormation() : Play() { }; } -uint8_t DefensiveStopFormation::score(const rtt::Field& field) noexcept { +uint8_t DefensiveStopFormation::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp index 59978d82f..5a363af7a 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp @@ -39,7 +39,7 @@ FreeKickThem::FreeKickThem() : Play() { }; } -uint8_t FreeKickThem::score(const rtt::Field& field) noexcept { +uint8_t FreeKickThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 665018b02..536142d55 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -72,7 +72,7 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); - PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo.passLocation); + PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); if (!ballKicked()) { stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp b/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp index 1891ef89a..47a26592d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp @@ -28,7 +28,7 @@ Halt::Halt() : Play() { std::make_unique("halt_10")}; } -uint8_t Halt::score(const rtt::Field &field) noexcept { +uint8_t Halt::score(const rtt::Field &) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp index 863654f11..8df4b1330 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp @@ -37,7 +37,7 @@ KickOffThem::KickOffThem() : Play() { }; } -uint8_t KickOffThem::score(const rtt::Field& field) noexcept { +uint8_t KickOffThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp index 27921db66..4ed59fe7b 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp @@ -35,7 +35,7 @@ KickOffThemPrepare::KickOffThemPrepare() : Play() { }; } -uint8_t KickOffThemPrepare::score(const rtt::Field& field) noexcept { +uint8_t KickOffThemPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index 01497ed11..2015af31a 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -32,7 +32,7 @@ KickOffUs::KickOffUs() : Play() { std::make_unique("halt_7")}; } -uint8_t KickOffUs::score(const rtt::Field &field) noexcept { +uint8_t KickOffUs::score(const rtt::Field &) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index d0ba0ae0d..5166f5378 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -36,7 +36,7 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { }; } -uint8_t KickOffUsPrepare::score(const rtt::Field& field) noexcept { +uint8_t KickOffUsPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp index 3df5f0313..6392e11b1 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp @@ -29,7 +29,7 @@ PenaltyThem::PenaltyThem() : Play() { std::make_unique("halt_9")}; } -uint8_t PenaltyThem::score(const rtt::Field& field) noexcept { +uint8_t PenaltyThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp index ec9cf6f06..6b72b1b75 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp @@ -34,7 +34,7 @@ PenaltyThemPrepare::PenaltyThemPrepare() : Play() { }; } -uint8_t PenaltyThemPrepare::score(const rtt::Field& field) noexcept { +uint8_t PenaltyThemPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp index 87e46cdc0..a319a79a1 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -32,7 +32,7 @@ PenaltyUs::PenaltyUs() : Play() { std::make_unique("halt_8")}; } -uint8_t PenaltyUs::score(const rtt::Field& field) noexcept { +uint8_t PenaltyUs::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp index f797497a4..4d37c6b11 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp @@ -34,7 +34,7 @@ PenaltyUsPrepare::PenaltyUsPrepare() : Play() { }; } -uint8_t PenaltyUsPrepare::score(const rtt::Field& field) noexcept { +uint8_t PenaltyUsPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return control_constants::FUZZY_TRUE; } diff --git a/roboteam_ai/src/stp/skills/Chip.cpp b/roboteam_ai/src/stp/skills/Chip.cpp index 5a086ec2e..8d962c510 100644 --- a/roboteam_ai/src/stp/skills/Chip.cpp +++ b/roboteam_ai/src/stp/skills/Chip.cpp @@ -38,7 +38,7 @@ Status Chip::onUpdate(const StpInfo &info) noexcept { command.id = info.getRobot().value()->getId(); // forward the generated command to the ControlModule, for checking and limiting - forwardRobotCommand(info.getCurrentWorld()); + forwardRobotCommand(); if (info.getBall()->get()->velocity.length() > stp::control_constants::HAS_CHIPPED_ERROR_MARGIN) { chipAttempts = 0; diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index 80f5f84da..98ccbe188 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -62,7 +62,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { command.id = info.getRobot().value()->getId(); // forward the generated command to the ControlModule, for checking and limiting - forwardRobotCommand(info.getCurrentWorld()); + forwardRobotCommand(); // Check if successful if ((info.getRobot().value()->getPos() - targetPos).length() <= stp::control_constants::GO_TO_POS_ERROR_MARGIN || diff --git a/roboteam_ai/src/stp/skills/Kick.cpp b/roboteam_ai/src/stp/skills/Kick.cpp index 21893b85c..e0fb90904 100644 --- a/roboteam_ai/src/stp/skills/Kick.cpp +++ b/roboteam_ai/src/stp/skills/Kick.cpp @@ -37,7 +37,7 @@ Status Kick::onUpdate(const StpInfo &info) noexcept { command.id = info.getRobot().value()->getId(); // forward the generated command to the ControlModule, for checking and limiting - forwardRobotCommand(info.getCurrentWorld()); + forwardRobotCommand(); if (!info.getRobot().value()->hasBall() && info.getBall()->get()->velocity.length() > control_constants::BALL_GOT_SHOT_LIMIT) { return Status::Success; diff --git a/roboteam_ai/src/stp/skills/Orbit.cpp b/roboteam_ai/src/stp/skills/Orbit.cpp index e0d1f9002..90684b268 100644 --- a/roboteam_ai/src/stp/skills/Orbit.cpp +++ b/roboteam_ai/src/stp/skills/Orbit.cpp @@ -42,7 +42,7 @@ Status Orbit::onUpdate(const StpInfo &info) noexcept { command.id = info.getRobot().value()->getId(); // forward the generated command to the ControlModule, for checking and limiting - forwardRobotCommand(info.getCurrentWorld()); + forwardRobotCommand(); // Check if successful double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; diff --git a/roboteam_ai/src/stp/skills/OrbitAngular.cpp b/roboteam_ai/src/stp/skills/OrbitAngular.cpp index 8c050510a..5052a6b07 100644 --- a/roboteam_ai/src/stp/skills/OrbitAngular.cpp +++ b/roboteam_ai/src/stp/skills/OrbitAngular.cpp @@ -27,7 +27,7 @@ Status OrbitAngular::onUpdate(const StpInfo &info) noexcept { command.targetAngle = targetAngle; command.dribblerSpeed = stp::control_constants::MAX_DRIBBLER_CMD; - forwardRobotCommand(info.getCurrentWorld()); // Send the robot command + forwardRobotCommand(); // Send the robot command // Return success if the angle difference is within the margin if (currentAngle.shortestAngleDiff(targetAngle) < stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI) { diff --git a/roboteam_ai/src/stp/skills/Rotate.cpp b/roboteam_ai/src/stp/skills/Rotate.cpp index a185edcc8..108994fac 100644 --- a/roboteam_ai/src/stp/skills/Rotate.cpp +++ b/roboteam_ai/src/stp/skills/Rotate.cpp @@ -26,7 +26,7 @@ Status Rotate::onUpdate(const StpInfo &info) noexcept { command.id = info.getRobot().value()->getId(); // forward the generated command to the ControlModule, for checking and limiting - forwardRobotCommand(info.getCurrentWorld()); + forwardRobotCommand(); // Check if the robot is within the error margin double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; diff --git a/roboteam_ai/src/stp/skills/TestSkill.cpp b/roboteam_ai/src/stp/skills/TestSkill.cpp index 371f30fb0..90bbbe1dc 100644 --- a/roboteam_ai/src/stp/skills/TestSkill.cpp +++ b/roboteam_ai/src/stp/skills/TestSkill.cpp @@ -6,7 +6,7 @@ namespace rtt::ai::stp::skill { -Status TestSkill::onUpdate(const StpInfo &info) noexcept { return Status::Running; } +Status TestSkill::onUpdate(const StpInfo &) noexcept { return Status::Running; } const char *TestSkill::getName() { return "TestSkill"; } diff --git a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp index 1df3dd5b9..f65983343 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -43,7 +43,7 @@ std::optional KeeperBlockBall::calculateInfoForSkill(StpInfo const &inf bool KeeperBlockBall::isEndTactic() noexcept { return true; } -bool KeeperBlockBall::isTacticFailing(const StpInfo &info) noexcept { return false; } +bool KeeperBlockBall::isTacticFailing(const StpInfo &) noexcept { return false; } bool KeeperBlockBall::shouldTacticReset(const StpInfo &info) noexcept { double errorMargin = control_constants::GO_TO_POS_ERROR_MARGIN; diff --git a/roboteam_ai/src/stp/tactics/TestTactic.cpp b/roboteam_ai/src/stp/tactics/TestTactic.cpp index 9af243f3b..e9447017d 100644 --- a/roboteam_ai/src/stp/tactics/TestTactic.cpp +++ b/roboteam_ai/src/stp/tactics/TestTactic.cpp @@ -19,9 +19,9 @@ std::optional TestTactic::calculateInfoForSkill(StpInfo const &info) no return skillStpInfo; } -bool TestTactic::isTacticFailing(const StpInfo &info) noexcept { return false; } +bool TestTactic::isTacticFailing(const StpInfo &) noexcept { return false; } -bool TestTactic::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool TestTactic::shouldTacticReset(const StpInfo &) noexcept { return false; } bool TestTactic::isEndTactic() noexcept { // This is not an end tactic diff --git a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp b/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp index 2ae93a7ce..2f95ee34c 100644 --- a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp +++ b/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp @@ -30,7 +30,7 @@ std::optional ChipAtPos::calculateInfoForSkill(StpInfo const &info) noe // Calculate the distance and the chip force double distanceBallToTarget = (info.getBall()->get()->position - info.getPositionToShootAt().value()).length(); - skillStpInfo.setKickChipVelocity(control::ControlUtils::determineChipForce(distanceBallToTarget, skillStpInfo.getShotType())); + skillStpInfo.setKickChipVelocity(control::ControlUtils::determineChipForce(distanceBallToTarget)); // When rotating, we need to dribble to keep the ball, but when chipping we don't if (skills.current_num() == 0) { diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index 3bd6d56a9..1b5c21b3c 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -46,7 +46,7 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc return skillStpInfo; } -bool GetBall::isTacticFailing(const StpInfo &info) noexcept { return false; } +bool GetBall::isTacticFailing(const StpInfo &) noexcept { return false; } bool GetBall::shouldTacticReset(const StpInfo &info) noexcept { return (!info.getRobot()->get()->hasBall() && skills.current_num() == 1); } diff --git a/roboteam_ai/src/stp/tactics/active/KickAtPos.cpp b/roboteam_ai/src/stp/tactics/active/KickAtPos.cpp index 2840a6e7f..f0fa227d3 100644 --- a/roboteam_ai/src/stp/tactics/active/KickAtPos.cpp +++ b/roboteam_ai/src/stp/tactics/active/KickAtPos.cpp @@ -47,7 +47,7 @@ bool KickAtPos::isTacticFailing(const StpInfo &info) noexcept { return false; } -bool KickAtPos::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool KickAtPos::shouldTacticReset(const StpInfo &) noexcept { return false; } const char *KickAtPos::getName() { return "Kick At Pos"; } diff --git a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp index a8ea63362..82354b87e 100644 --- a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp +++ b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp @@ -44,7 +44,7 @@ bool OrbitKick::isTacticFailing(const StpInfo &info) noexcept { return false; } -bool OrbitKick::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool OrbitKick::shouldTacticReset(const StpInfo &) noexcept { return false; } const char *OrbitKick::getName() { return "Orbit Kick"; } diff --git a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp index 6ee894f27..5b7bb4fe9 100644 --- a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp @@ -46,7 +46,7 @@ bool BallStandBack::isTacticFailing(const StpInfo &info) noexcept { return false; } -bool BallStandBack::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool BallStandBack::shouldTacticReset(const StpInfo &) noexcept { return false; } bool BallStandBack::isEndTactic() noexcept { // BallStandBack tactic is an end tactic diff --git a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp index df1f746d8..49af68b59 100644 --- a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp @@ -39,9 +39,9 @@ std::optional BlockBall::calculateInfoForSkill(StpInfo const &info) noe bool BlockBall::isEndTactic() noexcept { return true; } -bool BlockBall::isTacticFailing(const StpInfo &info) noexcept { return false; } +bool BlockBall::isTacticFailing(const StpInfo &) noexcept { return false; } -bool BlockBall::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool BlockBall::shouldTacticReset(const StpInfo &) noexcept { return false; } const char *BlockBall::getName() { return "Block Ball"; } diff --git a/roboteam_ai/src/stp/tactics/passive/Formation.cpp b/roboteam_ai/src/stp/tactics/passive/Formation.cpp index 7fd55d2cf..a97849781 100644 --- a/roboteam_ai/src/stp/tactics/passive/Formation.cpp +++ b/roboteam_ai/src/stp/tactics/passive/Formation.cpp @@ -33,7 +33,7 @@ bool Formation::isTacticFailing(const StpInfo &info) noexcept { return !info.getPositionToMoveTo(); } -bool Formation::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool Formation::shouldTacticReset(const StpInfo &) noexcept { return false; } bool Formation::isEndTactic() noexcept { // Formation tactic is an end tactic diff --git a/roboteam_ai/src/stp/tactics/passive/Halt.cpp b/roboteam_ai/src/stp/tactics/passive/Halt.cpp index 3a2d48768..d204bd5c9 100644 --- a/roboteam_ai/src/stp/tactics/passive/Halt.cpp +++ b/roboteam_ai/src/stp/tactics/passive/Halt.cpp @@ -18,9 +18,9 @@ std::optional Halt::calculateInfoForSkill(StpInfo const &info) noexcept return skillInfo; } -bool Halt::isTacticFailing(const StpInfo &info) noexcept { return false; } +bool Halt::isTacticFailing(const StpInfo &) noexcept { return false; } -bool Halt::shouldTacticReset(const StpInfo &info) noexcept { return false; } +bool Halt::shouldTacticReset(const StpInfo &) noexcept { return false; } bool Halt::isEndTactic() noexcept { // This is an end tactic diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 18efe8a7b..2fab7a3ca 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -270,13 +270,11 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro distance = robot->getPos().dist(*target_position); } - return costForDistance(distance, field->playArea.width(), field->playArea.height()); + return costForDistance(distance, field->playArea.height()); } // TODO these values need to be tuned. double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::DealerFlag &flag) { - auto fieldHeight = field->playArea.height(); - auto fieldWidth = field->playArea.width(); switch (flag.title) { // case DealerFlagTitle::WITH_WORKING_BALL_SENSOR: // return costForProperty(robot->isWorkingBallSensor()); @@ -310,13 +308,10 @@ void Dealer::setGameStateRoleIds(std::unordered_map o if (output.find("keeper") != output.end()) { interface::Output::setKeeperId(output.find("keeper")->second->getId()); } - if (output.find("ball_placer") != output.end()) { - interface::Output::setBallPlacerId(output.find("ball_placer")->second->getId()); - } } // Calculate the cost for distance. The further away the target, the higher the cost for that distance. -double Dealer::costForDistance(double distance, double fieldWidth, double fieldHeight) { +double Dealer::costForDistance(double distance, double fieldHeight) { auto fieldDiagonalLength = sqrt(pow(fieldHeight, 2.0) + pow(fieldHeight, 2.0)); return distance / fieldDiagonalLength; } diff --git a/roboteam_ai/src/utilities/IOManager.cpp b/roboteam_ai/src/utilities/IOManager.cpp index 4e384cbf1..01ba18255 100644 --- a/roboteam_ai/src/utilities/IOManager.cpp +++ b/roboteam_ai/src/utilities/IOManager.cpp @@ -96,8 +96,8 @@ void IOManager::addCameraAngleToRobotCommands(rtt::RobotCommands& robotCommands) const auto world = getState().last_seen_world(); const auto robots = rtt::GameSettings::isYellow() ? world.yellow() : world.blue(); for (auto& robotCommand : robotCommands) { - for (const auto robot : robots) { - if (robot.id() == robotCommand.id) { + for (const auto& robot : robots) { + if (robot.id() == static_cast(robotCommand.id)) { robotCommand.cameraAngleOfRobot = robot.angle(); robotCommand.cameraAngleOfRobotIsSet = true; } diff --git a/roboteam_ai/src/world/FieldComputations.cpp b/roboteam_ai/src/world/FieldComputations.cpp index 5a4531680..b3f0e5dbb 100644 --- a/roboteam_ai/src/world/FieldComputations.cpp +++ b/roboteam_ai/src/world/FieldComputations.cpp @@ -281,7 +281,7 @@ Vector2 FieldComputations::projectPointOutOfDefenseArea(const Field &field, Vect return {point.x, point.y + yDiff}; } -Vector2 FieldComputations::projectPointToValidPosition(const Field &field, Vector2 point, stp::AvoidObjects avoidObjects, double fieldMargin) { +Vector2 FieldComputations::projectPointToValidPosition(const Field &field, Vector2 point, stp::AvoidObjects avoidObjects) { Vector2 projectedPos = point; if (avoidObjects.shouldAvoidOutOfField) projectedPos = projectPointInField(field, projectedPos); if (avoidObjects.shouldAvoidDefenseArea) projectedPos = projectPointOutOfDefenseArea(field, projectedPos); @@ -302,7 +302,7 @@ Vector2 FieldComputations::projectPointIntoFieldOnLine(const Field &field, Vecto return (dist_lhs < dist_rhs ? *intersection_lhs : *intersection_rhs); // return the intersection closest to the point } -Vector2 FieldComputations::projectPointToValidPositionOnLine(const Field &field, Vector2 point, Vector2 p1, Vector2 p2, stp::AvoidObjects avoidObjects, double fieldMargin) { +Vector2 FieldComputations::projectPointToValidPositionOnLine(const Field &field, Vector2 point, Vector2 p1, Vector2 p2, double fieldMargin) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = getDefenseAreaMargin(); // Subtract PROJECTION_MARGIN to avoid the situation where the point is between the left field line and our goal line (-6.0 < x < -5.9) auto pointProjectedInField = projectPointIntoFieldOnLine(field, point, p1, p2, fieldMargin - PROJECTION_MARGIN); diff --git a/roboteam_ai/test/StpTests/TacticTests.cpp b/roboteam_ai/test/StpTests/TacticTests.cpp index 873ef4601..fda33b9f8 100644 --- a/roboteam_ai/test/StpTests/TacticTests.cpp +++ b/roboteam_ai/test/StpTests/TacticTests.cpp @@ -10,7 +10,7 @@ using namespace rtt::ai::stp; class TestSkill : public Skill { protected: - Status onUpdate(StpInfo const &info) noexcept override { return Status::Failure; } + Status onUpdate(StpInfo const &) noexcept override { return Status::Failure; } const char *getName() override { return "Test Skill"; } }; @@ -25,7 +25,7 @@ class MockTactic : public Tactic { } } - std::optional calculateInfoForSkill(const StpInfo &info) noexcept override { return StpInfo(); } + std::optional calculateInfoForSkill(const StpInfo &) noexcept override { return StpInfo(); } MOCK_METHOD1(isTacticFailingMock, bool(const StpInfo &)); From 121049e16b0b94bb2750cec8d8e3b8ab445f5002 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 10:40:42 +0100 Subject: [PATCH 2/8] Remove build warnings from logging, netwroking, robothub and utils --- roboteam_logging/test/LoggingTest.cpp | 2 +- .../system_tests/SendRobotCommands.cpp | 7 ++--- roboteam_robothub/include/RobotHub.h | 4 +-- .../scripts/sendFormationToSimulator.cpp | 2 +- roboteam_robothub/src/RobotHub.cpp | 26 +++++++++---------- .../src/basestation/BasestationManager.cpp | 2 +- .../include/roboteam_utils/Field.hpp | 18 ++++++------- .../test/utils/CircularBufferTest.cpp | 2 +- roboteam_utils/test/utils/PositionTest.cpp | 7 +++-- roboteam_utils/test/utils/Vector2Test.cpp | 4 +-- 10 files changed, 34 insertions(+), 40 deletions(-) diff --git a/roboteam_logging/test/LoggingTest.cpp b/roboteam_logging/test/LoggingTest.cpp index 65d183070..83da6f171 100644 --- a/roboteam_logging/test/LoggingTest.cpp +++ b/roboteam_logging/test/LoggingTest.cpp @@ -49,7 +49,7 @@ TEST(logging_writing, open) { stream.read(reinterpret_cast(&testHeader), sizeof(TestHeader)); ASSERT_TRUE(stream.good()); ASSERT_TRUE(testHeader.version == rtt::LOGFILE_VERSION); - for (int i = 0; i < rtt::LOGFILE_HEADER_NAME_SIZE; ++i) { + for (size_t i = 0; i < rtt::LOGFILE_HEADER_NAME_SIZE; ++i) { ASSERT_TRUE(testHeader.name[i] == rtt::DEFAULT_LOGFILE_HEADER_NAME[i]); } ASSERT_TRUE(removeFile(file_name)); diff --git a/roboteam_networking/system_tests/SendRobotCommands.cpp b/roboteam_networking/system_tests/SendRobotCommands.cpp index 468559d61..a3002ef33 100644 --- a/roboteam_networking/system_tests/SendRobotCommands.cpp +++ b/roboteam_networking/system_tests/SendRobotCommands.cpp @@ -38,6 +38,8 @@ void onFeedback(const rtt::RobotsFeedback& robotsFeedback) { bool hasBall = feedback.dribblerSeesBall; std::cout << " - Robot " << id << " has " << (hasBall ? "" : "not ") << "the ball" << std::endl; + std::cout << " - Robot " << id << "'s Xsens is " << (xSensCalibrated ? "calibrated" : "not calibrated") << std::endl; + std::cout << " - Robot " << id << "'s ball sensor is " << (ballsensorworking ? "working" : "not working") << std::endl; } } @@ -56,11 +58,6 @@ int main() { auto commandsBluePub = std::make_unique(); auto commandsYellowPub = std::make_unique(); - auto feedbackSub = std::make_unique(onFeedback); - auto worldSub = std::make_unique(onWorld); - - auto teamCommand = getEmptyRobotCommandToAllRobots(); - // First, send settings message to robothub that it needs to forward messages to the basestation std::cout << "Started test..." << std::endl; std::this_thread::sleep_for(std::chrono::seconds(2)); diff --git a/roboteam_robothub/include/RobotHub.h b/roboteam_robothub/include/RobotHub.h index 006cb3286..8e2bacfb6 100644 --- a/roboteam_robothub/include/RobotHub.h +++ b/roboteam_robothub/include/RobotHub.h @@ -60,9 +60,9 @@ class RobotHub { void handleRobotFeedbackFromBasestation(const REM_RobotFeedback &feedback, rtt::Team team); bool sendRobotFeedback(const rtt::RobotsFeedback &feedback); - void handleRobotStateInfo(const REM_RobotStateInfo &robotStateInfo, rtt::Team team); + // void handleRobotStateInfo(const REM_RobotStateInfo &robotStateInfo, rtt::Team team); - void handleBasestationLog(const std::string &basestationLogMessage, rtt::Team team); + // void handleBasestationLog(const std::string &basestationLogMessage, rtt::Team team); void handleSimulationErrors(const std::vector &); }; diff --git a/roboteam_robothub/scripts/sendFormationToSimulator.cpp b/roboteam_robothub/scripts/sendFormationToSimulator.cpp index a89513ca9..82cf1247c 100644 --- a/roboteam_robothub/scripts/sendFormationToSimulator.cpp +++ b/roboteam_robothub/scripts/sendFormationToSimulator.cpp @@ -15,7 +15,7 @@ int main() { try { manager = std::make_unique(config); - } catch (FailedToBindPortException e) { + } catch (const FailedToBindPortException& e) { std::cout << "Error: " << e.what() << std::endl; return -1; } diff --git a/roboteam_robothub/src/RobotHub.cpp b/roboteam_robothub/src/RobotHub.cpp index b7128fb72..71cb28361 100644 --- a/roboteam_robothub/src/RobotHub.cpp +++ b/roboteam_robothub/src/RobotHub.cpp @@ -35,8 +35,8 @@ RobotHub::RobotHub(bool shouldLog, bool logInMarpleFormat) { this->basestationManager = std::make_unique(); this->basestationManager->setFeedbackCallback([&](const REM_RobotFeedback &feedback, rtt::Team color) { this->handleRobotFeedbackFromBasestation(feedback, color); }); - this->basestationManager->setRobotStateInfoCallback([&](const REM_RobotStateInfo &robotStateInfo, rtt::Team color) { this->handleRobotStateInfo(robotStateInfo, color); }); - this->basestationManager->setBasestationLogCallback([&](const std::string &log, rtt::Team color) { this->handleBasestationLog(log, color); }); + // this->basestationManager->setRobotStateInfoCallback([&](const REM_RobotStateInfo &robotStateInfo, rtt::Team color) { this->handleRobotStateInfo(robotStateInfo, color); }); + // this->basestationManager->setBasestationLogCallback([&](const std::string &log, rtt::Team color) { this->handleBasestationLog(log, color); }); // if (shouldLog) { this->logger = RobotHubLogger(logInMarpleFormat); } } @@ -79,8 +79,6 @@ void RobotHub::sendCommandsToSimulator(const rtt::RobotCommands &commands, rtt:: auto kickSpeed = static_cast(robotCommand.kickSpeed); float kickAngle = robotCommand.kickType == rtt::KickType::CHIP ? SIM_CHIPPER_ANGLE_DEGREES : 0.0f; float dribblerSpeed = static_cast(robotCommand.dribblerSpeed) * SIM_MAX_DRIBBLER_SPEED_RPM; // dribblerSpeed is range of 0 to 1 - auto xVelocity = static_cast(robotCommand.velocity.x); - auto yVelocity = static_cast(robotCommand.velocity.y); auto angularVelocity = static_cast(robotCommand.targetAngularVelocity); if (!robotCommand.useAngularVelocity) { @@ -267,14 +265,14 @@ bool RobotHub::sendRobotFeedback(const rtt::RobotsFeedback &feedback) { return bytesSent > 0; } -void RobotHub::handleRobotStateInfo(const REM_RobotStateInfo &info, rtt::Team team) { - // if (this->logger.has_value()) { this->logger->logRobotStateInfo(info, team); } -} +// void RobotHub::handleRobotStateInfo(const REM_RobotStateInfo &info, rtt::Team team) { +// // if (this->logger.has_value()) { this->logger->logRobotStateInfo(info, team); } +// } -void RobotHub::handleBasestationLog(const std::string &basestationLogMessage, rtt::Team team) { - // if (this->logger.has_value()) { this->logger->logInfo("[" + teamToString(team) + "] " + basestationLogMessage); } - // RTT_INFO("Basestation ", teamToString(team), ": ", basestationLogMessage) -} +// void RobotHub::handleBasestationLog(const std::string &basestationLogMessage, rtt::Team team) { +// // if (this->logger.has_value()) { this->logger->logInfo("[" + teamToString(team) + "] " + basestationLogMessage); } +// // RTT_INFO("Basestation ", teamToString(team), ": ", basestationLogMessage) +// } void RobotHub::handleSimulationErrors(const std::vector &errors) { for (const auto &error : errors) { @@ -298,11 +296,11 @@ const char *FailedToInitializeNetworkersException::what() const noexcept { retur } // namespace rtt::robothub int main(int argc, char *argv[]) { - auto itLog = std::find(argv, argv + argc, std::string("-log")); + // auto itLog = std::find(argv, argv + argc, std::string("-log")); // bool shouldLog = itLog != argv + argc; - auto itMarple = std::find(argv, argv + argc, std::string("-marple")); - bool logForMarple = itMarple != argv + argc; + // auto itMarple = std::find(argv, argv + argc, std::string("-marple")); + // bool logForMarple = itMarple != argv + argc; // if (logForMarple) shouldLog = true; // Log for marple means to log diff --git a/roboteam_robothub/src/basestation/BasestationManager.cpp b/roboteam_robothub/src/basestation/BasestationManager.cpp index 4b40b94ff..dcb07fee2 100644 --- a/roboteam_robothub/src/basestation/BasestationManager.cpp +++ b/roboteam_robothub/src/basestation/BasestationManager.cpp @@ -117,7 +117,7 @@ void BasestationManager::handleIncomingMessage(const BasestationMessage& message return; } - if (message.payloadSize != payloadSize) { + if (static_cast(message.payloadSize) != payloadSize) { RTT_ERROR("Payload size of message does not match the size specified in the packet header. Received size: ", message.payloadSize, ", indicated size: ", payloadSize); return; } diff --git a/roboteam_utils/include/roboteam_utils/Field.hpp b/roboteam_utils/include/roboteam_utils/Field.hpp index 2acf8290c..48cef7957 100644 --- a/roboteam_utils/include/roboteam_utils/Field.hpp +++ b/roboteam_utils/include/roboteam_utils/Field.hpp @@ -28,15 +28,15 @@ typedef struct Field { FastRectangle rightGoalArea; // Right goal area outside the play area // TODO: Put these grids into a single grid of grids - Grid topLeftGrid; - Grid topMidGrid; - Grid topRightGrid; - Grid middleLeftGrid; - Grid middleMidGrid; - Grid middleRightGrid; - Grid bottomLeftGrid; - Grid bottomMidGrid; - Grid bottomRightGrid; + Grid topLeftGrid = {}; + Grid topMidGrid = {}; + Grid topRightGrid = {}; + Grid middleLeftGrid = {}; + Grid middleMidGrid = {}; + Grid middleRightGrid = {}; + Grid bottomLeftGrid = {}; + Grid bottomMidGrid = {}; + Grid bottomRightGrid = {}; bool operator==(const Field& other) const; diff --git a/roboteam_utils/test/utils/CircularBufferTest.cpp b/roboteam_utils/test/utils/CircularBufferTest.cpp index c43e09de8..006d0c9ab 100644 --- a/roboteam_utils/test/utils/CircularBufferTest.cpp +++ b/roboteam_utils/test/utils/CircularBufferTest.cpp @@ -15,7 +15,7 @@ void access() { } void constAccess() { const circular_buffer s; - double x = s.at(2); + s.at(2); } TEST(CircularBufferTest, atThrowTest) { EXPECT_ANY_THROW(access()); diff --git a/roboteam_utils/test/utils/PositionTest.cpp b/roboteam_utils/test/utils/PositionTest.cpp index ee3f8283d..4eea24194 100644 --- a/roboteam_utils/test/utils/PositionTest.cpp +++ b/roboteam_utils/test/utils/PositionTest.cpp @@ -3,10 +3,9 @@ #include "roboteam_utils/Position.h" using namespace rtt; - -//#define ASSERT_ALL_EQ(p, x, y, rot) ASSERT_DOUBLE_EQ(p.x, x); \ -// ASSERT_DOUBLE_EQ(p.y, y); \ -// ASSERT_DOUBLE_EQ(p.rot, rot); +// #define ASSERT_ALL_EQ(p, x, y, rot) ASSERT_DOUBLE_EQ(p.x, x); +// #define ASSERT_DOUBLE_EQ(p.y, y); +// #define ASSERT_DOUBLE_EQ(p.rot, rot); void ASSERT_ALL_EQ(Position p, double x, double y, double rot) { ASSERT_DOUBLE_EQ(p.x, x); ASSERT_DOUBLE_EQ(p.y, y); diff --git a/roboteam_utils/test/utils/Vector2Test.cpp b/roboteam_utils/test/utils/Vector2Test.cpp index 9edc36432..6638c52ef 100644 --- a/roboteam_utils/test/utils/Vector2Test.cpp +++ b/roboteam_utils/test/utils/Vector2Test.cpp @@ -197,8 +197,8 @@ TEST(VectorTests, lerp) { EXPECT_DOUBLE_EQ(origin.x, 0); EXPECT_DOUBLE_EQ(origin.y, 0); - EXPECT_DOUBLE_EQ(origin.x, 0); - EXPECT_DOUBLE_EQ(origin.y, 0); + EXPECT_DOUBLE_EQ(origin2.x, 0); + EXPECT_DOUBLE_EQ(origin2.y, 0); } TEST(VectorTests, nan) { From 4bc419f318b7c543a3e561e16c13a73c382d7d0b Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 10:45:53 +0100 Subject: [PATCH 3/8] Remove build warning from velocity Estimate --- .../include/observer/filters/vision/robot/CameraRobotFilter.h | 2 +- .../observer/src/filters/vision/robot/CameraRobotFilter.cpp | 2 +- .../observer/src/filters/vision/robot/RobotFilter.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h b/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h index 819980946..bb67bfdb5 100644 --- a/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h +++ b/roboteam_world/observer/include/observer/filters/vision/robot/CameraRobotFilter.h @@ -24,7 +24,7 @@ class CameraRobotFilter : public CameraObjectFilter { [[nodiscard]] FilteredRobot estimate(const Time& time) const; - [[nodiscard]] RobotVel velocityEstimate(const Time& time) const; + [[nodiscard]] RobotVel velocityEstimate() const; [[nodiscard]] bool acceptObservation(const RobotObservation& observation) const; diff --git a/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp b/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp index 30f9f2c35..058fb3bf8 100644 --- a/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/robot/CameraRobotFilter.cpp @@ -80,7 +80,7 @@ bool CameraRobotFilter::acceptObservation(const RobotObservation &observation) c return posDifSq < 0.4 * 0.4 && angleDif < M_PI_2; // TODO: remove hardcoded constants } -RobotVel CameraRobotFilter::velocityEstimate(const Time &time) const { +RobotVel CameraRobotFilter::velocityEstimate() const { Eigen::Vector2d vel = positionFilter.getVelocity(); double angVel = angleFilter.getVelocity(); return RobotVel(vel, angVel); diff --git a/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp b/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp index 246a9ec0a..ffe75b4f9 100644 --- a/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/robot/RobotFilter.cpp @@ -28,7 +28,7 @@ bool RobotFilter::processDetection(const RobotObservation &observation) { // TODO: make this a weighted average (using e.g. filter age / health?) RobotVel velocity = RobotVel(); for (const auto &filter : cameraFilters) { - velocity += filter.second.velocityEstimate(observation.timeCaptured); + velocity += filter.second.velocityEstimate(); } velocity /= double(cameraFilters.size()); cameraFilters.insert(std::make_pair(observation.cameraID, CameraRobotFilter(observation, velocity))); From 3eb5fd0d9b744752c6988118f3d4a81ec0860774 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 11:09:37 +0100 Subject: [PATCH 4/8] Remove build warnings from world --- .../observer/filters/vision/VisionFilter.h | 7 +++---- .../observer/filters/vision/WorldFilter.h | 7 ------- .../src/filters/vision/VisionFilter.cpp | 17 +++++------------ .../observer/src/filters/vision/WorldFilter.cpp | 3 --- 4 files changed, 8 insertions(+), 26 deletions(-) diff --git a/roboteam_world/observer/include/observer/filters/vision/VisionFilter.h b/roboteam_world/observer/include/observer/filters/vision/VisionFilter.h index 75b94d20f..704027ae5 100644 --- a/roboteam_world/observer/include/observer/filters/vision/VisionFilter.h +++ b/roboteam_world/observer/include/observer/filters/vision/VisionFilter.h @@ -45,14 +45,13 @@ class VisionFilter { /** * processes any geometry data and passes it to the * @param packets the packets to check for geometry changes - * @return True if the geometry was somehow changed/updated and different + * @return void */ - bool processGeometry(const std::vector& packets); + void processGeometry(const std::vector& packets); /** * @param packets the relevant detection frames from SSL-vision - * @param update_geometry Set this to true to update the Geometry used by the world filter */ - void processDetections(const std::vector& packets, bool update_geometry, const std::vector& robotData); + void processDetections(const std::vector& packets, const std::vector& robotData); GeometryFilter geomFilter; WorldFilter worldFilter; diff --git a/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h b/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h index 15711d8cb..78d2ed323 100644 --- a/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h +++ b/roboteam_world/observer/include/observer/filters/vision/WorldFilter.h @@ -28,13 +28,6 @@ class WorldFilter { [[nodiscard]] proto::World getWorldPrediction(const Time& time) const; - /** - * Updates the cameras which the worldFilter uses for calculations, and the field geometry which is used for detecting - * wall collisions. - * @param geometry to grab the cameras from - */ - void updateGeometry(const proto::SSL_GeometryData& geometry); - void updateRobotParameters(const TwoTeamRobotParameters& robotInfo); private: diff --git a/roboteam_world/observer/src/filters/vision/VisionFilter.cpp b/roboteam_world/observer/src/filters/vision/VisionFilter.cpp index bb0e8c4c3..aebc886fc 100644 --- a/roboteam_world/observer/src/filters/vision/VisionFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/VisionFilter.cpp @@ -4,29 +4,22 @@ #include "filters/vision/VisionFilter.h" proto::World VisionFilter::process(const std::vector& packets, const std::vector& robotData) { - bool geometry_updated = processGeometry(packets); - processDetections(packets, geometry_updated, robotData); + processGeometry(packets); + processDetections(packets, robotData); // TODO for now not extrapolating because grsim sends packets from 1970... Time extroplatedToTime = getExtrapolationTimeForPolicy(); return worldFilter.getWorldPrediction(extroplatedToTime); } -bool VisionFilter::processGeometry(const std::vector& packets) { - bool newGeometry = false; +void VisionFilter::processGeometry(const std::vector& packets) { for (const auto& packet : packets) { if (packet.has_geometry()) { - if (geomFilter.process(packet.geometry())) { - newGeometry = true; - } + geomFilter.process(packet.geometry()); } } - return newGeometry; } -void VisionFilter::processDetections(const std::vector& packets, bool update_geometry, const std::vector& robotData) { - if (update_geometry) { - worldFilter.updateGeometry(geomFilter.getGeometry()); - } +void VisionFilter::processDetections(const std::vector& packets, const std::vector& robotData) { std::vector detectionFrames; for (const auto& packet : packets) { if (packet.has_detection()) { diff --git a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp index 8d9ce0936..4606afff4 100644 --- a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp @@ -7,9 +7,6 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} -void WorldFilter::updateGeometry(const proto::SSL_GeometryData &geometry) { - // TODO: fix -} proto::World WorldFilter::getWorldPrediction(const Time &time) const { proto::World world; From 65edf19e99f42cd0bc79a5a865195ed8c13cb7ea Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 11:12:55 +0100 Subject: [PATCH 5/8] Formatting --- .../roboteam_ai/control/positionControl/PositionControl.h | 4 ++-- .../control/positionControl/BBTrajectories/WorldObjects.cpp | 5 +++-- .../src/control/positionControl/pathTracking/PidTracking.cpp | 2 +- roboteam_world/observer/src/filters/vision/WorldFilter.cpp | 1 - 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index b2c9db3c6..d68d2ea66 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -116,8 +116,8 @@ class PositionControl { * @param startTime the time at which the trajectory starts * @return A score for the trajectory */ - double calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional &firstCollision, - Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime = 0); + double calculateScore(const rtt::world::World *world, const rtt::Field &field, std::optional &firstCollision, Trajectory2D &trajectoryAroundCollision, + stp::AvoidObjects avoidObjects, double startTime = 0); /** * @brief Tries to find a new trajectory when the current path has a collision on it. It tries this by diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 64aeb4e36..5b174f8f9 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -199,7 +199,7 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s int numSteps = static_cast(maxCollisionCheckTime / timeStep); for (int i = 0; i <= numSteps; ++i, completedTimeSteps[robotId]++) { double loopTime = i * timeStep; - if (static_cast(completedTimeSteps[robotId] + 1) >= pathPoints.size()) { + if (static_cast(completedTimeSteps[robotId] + 1) >= pathPoints.size()) { return; } const double startPointX_OurRobot = pathPoints[completedTimeSteps[robotId]].x; @@ -223,7 +223,8 @@ void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, s double startPointX_OtherRobot, startPointY_OtherRobot, velocityX_OtherRobot, velocityY_OtherRobot; - if (completedTimeStepsIt != completedTimeSteps.end() && computedPathsIt != computedPaths.end() && static_cast(completedTimeStepsIt->second + 1) < computedPathsIt->second.size()) { + if (completedTimeStepsIt != completedTimeSteps.end() && computedPathsIt != computedPaths.end() && + static_cast(completedTimeStepsIt->second + 1) < computedPathsIt->second.size()) { startPointX_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].x; startPointY_OtherRobot = computedPathsIt->second[completedTimeStepsIt->second].y; velocityX_OtherRobot = (computedPathsIt->second[completedTimeStepsIt->second + 1].x - startPointX_OtherRobot) / timeStep; diff --git a/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp b/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp index 32c3e8aed..2dca42fd8 100644 --- a/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp +++ b/roboteam_ai/src/control/positionControl/pathTracking/PidTracking.cpp @@ -2,7 +2,7 @@ namespace rtt::ai::control { -Position PidTracking::trackPath(const Vector2 ¤tPosition, const Vector2&, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) { +Position PidTracking::trackPath(const Vector2 ¤tPosition, const Vector2 &, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) { PositionControlUtils::removeFirstIfReached(pathPoints, currentPosition); if (pidMapping.find(robotId) == pidMapping.end()) { pidMapping[robotId] = std::make_pair(PID(), PID()); diff --git a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp index 4606afff4..4ecf7b247 100644 --- a/roboteam_world/observer/src/filters/vision/WorldFilter.cpp +++ b/roboteam_world/observer/src/filters/vision/WorldFilter.cpp @@ -7,7 +7,6 @@ #include "filters/vision/ball/BallAssigner.h" WorldFilter::WorldFilter() {} - proto::World WorldFilter::getWorldPrediction(const Time &time) const { proto::World world; addRobotPredictionsToMessage(world, time); From cd800363f881b61dfea223650a434d5ba7514034 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 17:35:49 +0100 Subject: [PATCH 6/8] Fix for failing tests --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 688a1c663..0afd7a2f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,3 +105,6 @@ add_subdirectory(roboteam_logging) add_subdirectory(roboteam_ai) add_subdirectory(roboteam_robothub) add_subdirectory(roboteam_world) + +# Ensure that the roboteam_robothub target is built after BuildLibusb is done +add_dependencies(roboteam_robothub project_libusb) \ No newline at end of file From 02f9b47489db1da5f4a64909b01c028777f31914 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 19:05:34 +0100 Subject: [PATCH 7/8] Fix for failing tests --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0afd7a2f6..7e951587b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,8 +103,8 @@ add_subdirectory(roboteam_networking) add_subdirectory(roboteam_utils) add_subdirectory(roboteam_logging) add_subdirectory(roboteam_ai) -add_subdirectory(roboteam_robothub) +# add_subdirectory(roboteam_robothub) add_subdirectory(roboteam_world) # Ensure that the roboteam_robothub target is built after BuildLibusb is done -add_dependencies(roboteam_robothub project_libusb) \ No newline at end of file +# add_dependencies(roboteam_robothub project_libusb) \ No newline at end of file From b7b2af777aacfd04db6b46f91afcbd0775ffba90 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 26 Jan 2024 23:49:43 +0100 Subject: [PATCH 8/8] Fix for failing tests --- CMakeLists.txt | 8 ++------ roboteam_robothub/CMakeLists.txt | 1 + 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e951587b..47fe0f2c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,6 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${RTT_BUILD_FOLDER}/lib) set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules) include(ccache) # For faster compiling include(GetZmqpp) # For networking between AI, RobotHub and World -include(BuildLibusb) # USB library for RobotHub include(CheckCXXCompilerFlag) # For testing compiler cxx standard support include(LocateQt5) # For GUI and some networking include(GetGoogleTest) # For testing @@ -103,8 +102,5 @@ add_subdirectory(roboteam_networking) add_subdirectory(roboteam_utils) add_subdirectory(roboteam_logging) add_subdirectory(roboteam_ai) -# add_subdirectory(roboteam_robothub) -add_subdirectory(roboteam_world) - -# Ensure that the roboteam_robothub target is built after BuildLibusb is done -# add_dependencies(roboteam_robothub project_libusb) \ No newline at end of file +add_subdirectory(roboteam_robothub) +add_subdirectory(roboteam_world) \ No newline at end of file diff --git a/roboteam_robothub/CMakeLists.txt b/roboteam_robothub/CMakeLists.txt index 3db856f44..19b79c4ec 100644 --- a/roboteam_robothub/CMakeLists.txt +++ b/roboteam_robothub/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(Qt5Network REQUIRED) set(CMAKE_THREAD_PREFER_PTHREAD TRUE) set(THREADS_PREFER_PTHREAD_FLAG TRUE) find_package(Threads REQUIRED) +include(BuildLibusb) # USB library for RobotHub # Generate the proto files for the simulator manager find_package(Protobuf 3.9.1 REQUIRED)