diff --git a/CMakeLists.txt b/CMakeLists.txt index ec848876fb..ce119b252d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,6 +56,7 @@ target_include_directories(roboteam_ai_skills PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_skills + PRIVATE Tracy::TracyClient PRIVATE roboteam_networking PRIVATE roboteam_utils PRIVATE Qt5::Widgets @@ -91,6 +92,7 @@ target_include_directories(roboteam_ai_tactics PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_tactics + PRIVATE Tracy::TracyClient PRIVATE roboteam_utils PRIVATE roboteam_networking PRIVATE Qt5::Widgets @@ -173,6 +175,7 @@ target_include_directories(roboteam_ai_plays PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_plays + PRIVATE Tracy::TracyClient PUBLIC roboteam_interface_utils_lib PRIVATE roboteam_networking PRIVATE roboteam_utils @@ -262,30 +265,21 @@ target_compile_options(roboteam_ai_computation PRIVATE "${COMPILER_FLAGS}") add_library(roboteam_ai_control ${PROJECT_SOURCE_DIR}/src/control/ControlModule.cpp ${PROJECT_SOURCE_DIR}/src/control/ControlUtils.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathPlanning/NumTreesPlanning.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/CollisionDetector.cpp ${PROJECT_SOURCE_DIR}/src/control/positionControl/PositionControl.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathPlanning/VoronoiPathPlanning.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathTracking/DensePathTracking.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathTracking/PidTracking.cpp ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathTracking/BBTPathTracking.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/PathPointNode.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathPlanning/PathPlanningAlgorithm.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathTracking/PathTrackingAlgorithm.cpp ${PROJECT_SOURCE_DIR}/src/control/positionControl/PositionControlUtils.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/WorldObjects.cpp ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/BBTrajectory1D.cpp ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/Trajectory1D.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/Trajectory2D.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/WorldObjects.cpp - ${PROJECT_SOURCE_DIR}/src/control/positionControl/BBTrajectories/WorldObjects.cpp ${PROJECT_SOURCE_DIR}/src/control/AnglePID.cpp -) + ${PROJECT_SOURCE_DIR}/src/control/positionControl/CollisionDetector.cpp + ${PROJECT_SOURCE_DIR}/src/control/positionControl/pathPlanning/BBTPathPlanning.cpp + ) + target_include_directories(roboteam_ai_control PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_control + PRIVATE Tracy::TracyClient PRIVATE roboteam_networking PRIVATE roboteam_utils PRIVATE Qt5::Widgets @@ -360,6 +354,7 @@ target_include_directories(roboteam_ai_world PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai_world + PRIVATE Tracy::TracyClient PRIVATE roboteam_networking PRIVATE roboteam_utils PRIVATE Qt5::Widgets @@ -375,6 +370,7 @@ target_include_directories(roboteam_ai PRIVATE include/roboteam_ai ) target_link_libraries(roboteam_ai + PRIVATE Tracy::TracyClient PRIVATE roboteam_utils PRIVATE Qt5::Widgets PRIVATE roboteam_networking diff --git a/include/roboteam_ai/control/positionControl/BBTrajectories/BBTrajectory2D.h b/include/roboteam_ai/control/positionControl/BBTrajectories/BBTrajectory2D.h index ef8b3f8e07..d5de1ad84a 100644 --- a/include/roboteam_ai/control/positionControl/BBTrajectories/BBTrajectory2D.h +++ b/include/roboteam_ai/control/positionControl/BBTrajectories/BBTrajectory2D.h @@ -10,9 +10,15 @@ #include #include "BBTrajectory1D.h" +#include "utilities/Constants.h" namespace rtt::BB { +struct PosVelVector { + Vector2 position; + Vector2 velocity; +}; + /** * @author Rolf * @brief Class that computes and stores 2 dimensional bang-bang trajectories. These can compute close to time-optimal @@ -34,21 +40,8 @@ class BBTrajectory2D { * @param maxVel The maximum allowed velocity for this path. * @param maxAcc The maximum allowed acceleration allowed for the robot on this path */ - BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc); - - /** - * @brief Computes a bang bang trajectory with a given alpha value. - * This is NOT time optimal and may give very 'unphysical' paths for high or low alpha value. - * Don't use this constructor unless you know what you are doing. - * @param initialPos The initial position to start the trajectory from - * @param initialVel The initial velocity to start the trajectory from - * @param finalPos The final position to arrive at. - * @param maxVel The maximum allowed velocity for this path. - * @param maxAcc The maximum allowed acceleration allowed for the robot on this path - * @param alpha The chosen angle, should be between 0 and M_PI_2. Angle 0 gives all of the control to the x dimension - * whilst at M_PI all of the velocity/acceleration is given to y dimension. - */ - BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double alpha); + BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, + double timeStep = ai::Constants::POSITION_CONTROL_TIME_STEP() / 1000.0); /** * @brief Computes a time-optimal bang-bang trajectory. @@ -83,19 +76,12 @@ class BBTrajectory2D { */ [[nodiscard]] Vector2 getAcceleration(double t) const; - /** - * @brief This function computes a straight line approximation that goes through all the points. - * This can be useful for e.g. visualization - * @return a vector with N positions spaced equally in time. - */ - [[nodiscard]] std::vector getStraightLines(unsigned int N) const; - /** * @brief Approaches the BangBangTrajectory by dividing the path in points which are separated by timeStep seconds * @param timeStep time between pathpoints * @return */ - [[nodiscard]] std::vector getPathApproach(double timeStep) const; + [[nodiscard]] std::vector &getPathApproach(); /** * @brief Gets tEnd of the current part @@ -105,12 +91,12 @@ class BBTrajectory2D { /** * @brief Returns a vector with all the velocities (Vector2) at specified timeSteps */ - [[nodiscard]] std::vector getVelocityVector(double timeStep) const; + [[nodiscard]] std::vector &getVelocityVector(); /** * @brief Transforms the BBTrajectory into a posVelVector at specified timeSteps */ - [[nodiscard]] std::vector> getPosVelVector(double timeStep); + [[nodiscard]] std::vector getPosVelVector(); /** * @brief Returns all the trajectory parts in both dimensions to use in the general trajectory class @@ -134,6 +120,10 @@ class BBTrajectory2D { BBTrajectory1D x; BBTrajectory1D y; + + double timeStep = ai::Constants::POSITION_CONTROL_TIME_STEP() / 1000.0; + std::vector positions; + std::vector velocities; }; } // namespace rtt::BB #endif // RTT_BBTRAJECTORY2D_H diff --git a/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory1D.h b/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory1D.h deleted file mode 100644 index dbf9e91fc9..0000000000 --- a/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory1D.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// Created by tijmen on 16-12-21. -// - -#ifndef RTT_TRAJECTORY1D_H -#define RTT_TRAJECTORY1D_H - -#include "BBTrajectory1D.h" - -namespace rtt { - -class Trajectory1D { - public: - void addTrajectory(const std::vector &newParts, double addFromTime); - - /** - * @brief Gets the position at time t - * @param t time to get position at - * @return Position at time t - */ - [[nodiscard]] double getPosition(double t) const; - - /** - * @brief Gets the velocity at time t - * @param t time to get velocity at - * @return Velocity at time t - */ - [[nodiscard]] double getVelocity(double t) const; - - /** - * @brief Gets the acceleration at time t - * @param t time to get acceleration at. - * @return Acceleration at time t - */ - [[nodiscard]] double getAcceleration(double t) const; - - /** - * @brief Gets the total trajectory time. - * @return Total time of the trajectory to end point - */ - [[nodiscard]] double getTotalTime() const; - - std::vector parts; - double initialPos; // m - double finalPos; // m - double initialVel; // m/s - double maxAcc; // m/s^2 - double maxVel; // m/s -}; - -} // namespace rtt - -#endif // RTT_TRAJECTORY1D_H diff --git a/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h b/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h deleted file mode 100644 index c876cfd736..0000000000 --- a/include/roboteam_ai/control/positionControl/BBTrajectories/Trajectory2D.h +++ /dev/null @@ -1,72 +0,0 @@ -// -// Created by tijmen on 15-12-21. -// - -#ifndef RTT_TRAJECTORY2D_H -#define RTT_TRAJECTORY2D_H - -#include - -#include "Trajectory1D.h" -#include "roboteam_utils/Vector2.h" - -namespace rtt { - -class Trajectory2D { - public: - /** - * @brief Default constructor - */ - Trajectory2D() = default; - - Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc); - - void addTrajectory(const Trajectory2D &extraTrajectory, double addFromTime); - - /** - * @brief Approaches the Trajectory by dividing the path in points which are separated by timeStep seconds - * @param timeStep time between pathpoints - * @return - */ - [[nodiscard]] std::vector getPathApproach(double timeStep) const; - - /** - * @brief Returns a vector with all the velocities (Vector2) at specified timeSteps - */ - [[nodiscard]] std::vector getVelocityVector(double timeStep) const; - - /** - * @brief Get the position in the trajectory at time t - * @param t Given time. - * @return Position at time t - */ - [[nodiscard]] Vector2 getPosition(double t) const; - - /** - * @brief Get the velocity in the trajectory at time t - * @param t Given time. - * @return Velocity at time t - */ - [[nodiscard]] Vector2 getVelocity(double t) const; - - /** - * @brief Get the acceleration in the trajectory at time t - * @param t Given time. - * @return Acceleration at time t - */ - [[nodiscard]] Vector2 getAcceleration(double t) const; - - /** - * @brief Gets the total trajectory time. - * @return Total time of the trajectory to end point - */ - [[nodiscard]] double getTotalTime() const; - - private: - Trajectory1D x; - Trajectory1D y; -}; - -} // namespace rtt - -#endif // RTT_TRAJECTORY2D_H diff --git a/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h b/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h deleted file mode 100644 index 2968b7e5d6..0000000000 --- a/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h +++ /dev/null @@ -1,147 +0,0 @@ -// -// Created by floris on 15-11-20. -// - -#ifndef RTT_WORLDOBJECTS_H -#define RTT_WORLDOBJECTS_H - -#include - -#include "control/positionControl/BBTrajectories/Trajectory2D.h" -#include "utilities/GameStateManager.hpp" -#include "utilities/StpInfoEnums.h" -#include "world/FieldComputations.h" - -namespace rtt::BB { - -/** - * @memberof obstaclePosition position of the obstacle - * @memberof collisionPosition position robot shouldn't come - * @memberof collisionTime number of seconds from now that the collision will occur - * @memberof collisionName the name of what causes the collision - */ -struct CollisionData { - Vector2 obstaclePosition; - Vector2 collisionPosition; - double collisionTime; - std::string collisionName; -}; - -/** - * @brief struct used for returning a command for pathtracking and - * information about a collision which can be used STP - * @memberof robotCommand - * @memberof collision - */ -struct CommandCollision { - RobotCommand robotCommand; - std::optional collisionData; -}; - -class WorldObjects { - private: - rtt::ai::GameStateManager gameStateManager; - rtt::ai::GameState gameState; - rtt::ai::RuleSet ruleset; - - public: - WorldObjects(); - - /** - * Takes a BangBangTrajectory of a robot and checks the path in certain intervals for collisions - * @brief Calculates the position of the first collision and the obstacle position on a BangBangTrajectory - * @param world the world object used for information about the robots - * @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 - * @param avoidObjects a struct with if it should avoid certain objects - * @return optional with rtt::BB::CollisionData - */ - std::optional getFirstCollision(const rtt::world::World *world, const rtt::world::Field &field, const rtt::Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects); - - /** - * Takes a discretized trajectory of a robot and checks the path in certain intervals for collisions - * @brief Calculates the position of the first collision and the obstacle position on a BangBangTrajectory - * @param world the world object used for information about the robots - * @param field used for checking collisions with the field - * @param BBTrajectory the discretized trajectory to check for collisions - * @param computedPaths the paths of our robots - * @param robotId - * @param timeStep - * @return optional with rtt::BB::CollisionData - */ - std::optional getFirstCollisionDiscretized(const rtt::world::World *world, const rtt::world::Field &field, - const std::vector> &discretizedTrajectoryPosVel, - const std::unordered_map> &computedPaths, int robotId, double timeStep); - - /** - * @brief Takes a calculated path of a robot and checks points along the path if they are outside the - * fieldlines if not allowed there. Adds these points and the time to collisionDatas and collisionTimes - * @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 - * @param timeStep in seconds - */ - void calculateFieldCollisions(const rtt::world::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, double timeStep); - - /** - * @brief Takes a calculated path and checks points along the path if they are inside the defensearea if - * the robot is not allowed to be there. Adds these points and the time to collisionDatas and collisionTimes - * @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 - * @param timeStep in seconds - */ - void calculateDefenseAreaCollisions(const rtt::world::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, - double timeStep); - - /** - * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an - * approximation of the ball trajactory. If the play is "ball_placement_them" also checks for the path - * being inside the balltube. Adds these points and the time to collisionDatas and collisionTimes - * @param world Used for information about the ball - * @param collisionDatas, std::vector which rtt::BB::CollisionData can be added to - * @param pathPoints, std::vector with path points - * @param timeStep in seconds - */ - void calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, std::vector pathPoints, double timeStep); - - /** - * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an - * approximation of the enemy robot paths. Adds these points and the time to collisionDatas and collisionTimes if - * the difference in velocity between the two robots is more than 1.5 ms/s and we are driving faster - * @param world Used for information about the enemy robots - * @param BBTrajectory BBTrajectory which is used for getting the velocity of the robot - * @param collisionDatas, std::vector which rtt::BB::CollisionData can be added to - * @param pathPoints, std::vector with path points - * @param timeStep in seconds - * @param timeStepsDone how many seconds of the trajectory is already completed by the robot - */ - void calculateEnemyRobotCollisions(const rtt::world::World *world, rtt::Trajectory2D Trajectory, std::vector &collisionDatas, - const std::vector &pathPoints, double timeStep, size_t timeStepsDone); - - /** - * @brief Takes a path from the array of stored paths and checks points along the path if they are too close to - * where our robots are calculated to be at that point in time. Adds these points and the time to collisionDatas - * and collisionTimes - * @param world Used for information about our robots - * @param collisionDatas, std::vector which rtt::BB::CollisionData can be added to - * @param pathPoints, std::vector with path points - * @param computedPaths The paths of our own robots - * @param robotId - * @param timeStep in seconds - * @param timeStepsDone how many seconds of the trajectory is already completed by the robot - */ - void calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t timeStepsDone); - - // Inserts collisionData in the vector collisionDatas such that they are ordered from lowest collisionTime to highest - void insertCollisionData(std::vector &collisionDatas, const CollisionData &collisionData); -}; -} // namespace rtt::BB - -#endif // RTT_WORLDOBJECTS_H diff --git a/include/roboteam_ai/control/positionControl/CollisionDetector.h b/include/roboteam_ai/control/positionControl/CollisionDetector.h index 84f75f505e..34061bdb32 100644 --- a/include/roboteam_ai/control/positionControl/CollisionDetector.h +++ b/include/roboteam_ai/control/positionControl/CollisionDetector.h @@ -1,77 +1,138 @@ // -// Created by ratoone on 10-12-19. +// Created by martin on 11-5-22. // -#ifndef RTT_COLLISIONDETECTOR_H -#define RTT_COLLISIONDETECTOR_H +#pragma once +#include +#include +#include + +#include "control/positionControl/BBTrajectories/BBTrajectory2D.h" #include "utilities/Constants.h" #include "world/Field.h" -#include "world/views/RobotView.hpp" +#include "world/FieldComputations.h" namespace rtt::ai::control { -/** - * Checks for collision between points and different components of the field: robots, defence area, etc. - * Check isCollisionBetweenPoints method for more details - */ +struct Obstacles { + BB::PosVelVector ball; + std::vector robotsThem; + std::unordered_map robotsUs; +}; + +struct Collision { + Vector2 position; + Vector2 velocity; + int timeStep; +}; + +template +concept PathPointType = std::is_same_v < std::decay_t, +Vector2 > || std::is_same_v, BB::PosVelVector>; + class CollisionDetector { private: - static constexpr double DEFAULT_ROBOT_COLLISION_RADIUS = 3.0 * Constants::ROBOT_RADIUS(); + static constexpr int TIME_STEP = ai::Constants::POSITION_CONTROL_TIME_STEP(); + static constexpr int STEP_COUNT = ai::Constants::POSITION_CONTROL_STEP_COUNT(); + static constexpr double MIN_ROBOT_DISTANCE = 3 * ai::Constants::ROBOT_RADIUS_MAX(); + double minBallDistance = 0.0; - std::vector robotPositions; - const rtt::world::Field* field = nullptr; + std::vector timeline; + std::optional field; + + /** + * @brief Consolidation of collision detection logic. + * @tparam pathPoint PathPoint to check for collisions. + * @param obstaclePos Obstacle to check against. + * @param minDistance Minimum distance between origin and obstacle. + * */ + template + [[nodiscard]] static bool isCollision(const T& pathPoint, const Vector2& obstaclePos, double minDistance); + + /** + * @brief Finds the first (time-wise) collision with dynamic obstacles(ball and robots) on the path + * @tparam pathPoints Trajectory approximation + * @param robotId Robot id to ignore (i.e it self) + * @param timeOffset Time offset to start checking from + * @param shouldAvoidBall Whether to avoid the ball or not + * @param timeLimit Time limit to check for collisions + */ + template + [[nodiscard]] std::optional getFirstObjectCollision(std::span path, int robotId, bool shouldAvoidBall, int timeOffset = 0, int timeLimit = STEP_COUNT) const; - public: /** - * Checks if a new point can be followed by a robot from a starting position. This implies having - * no collisions with other robots, the outside of the field, or the defence area - * @param initialPoint the starting point - * @param nextPoint the destination point - * @return true if there is no collision, false otherwise + * @brief Finds the first (time-wise) collision with the playing field. + * @tparam pathPoints Trajectory approximation */ - bool isCollisionBetweenPoints(const Vector2& initialPoint, const Vector2& nextPoint); + template + [[nodiscard]] std::optional getFirstFieldCollision(std::span path) const; /** - * Checks whether the line drawn by the two points comes close to any robot (excepting the current one) - * and returns that robot's position - * @param initialPoint - * @param nextPoint - * @param currentRobotPosition the current robot position (should be ignored when checking) - * @return the colliding robot position, or a std::nullopt if there is no collision + * @brief Finds the first (time-wise) collision with defense area. + * @tparam pathPoints Trajectory approximation */ - std::optional getRobotCollisionBetweenPoints(const Vector2& initialPoint, const Vector2& nextPoint); + template + [[nodiscard]] std::optional getFirstDefenseAreaCollision(std::span path) const; /** - * Check if the point is inside the field - * @param point the point to check - * @return true if the point is in the field + * @brief Unwrap the PathPointType to Vector2 at compile-time. + * @tparam container PathPoint to unwrap. */ - bool isPointInsideField(const Vector2& point); + template + [[nodiscard]] static constexpr const Vector2& unwrapPosition(const T& container); + + public: + CollisionDetector(); + + /** + * @brief Finds the first (time-wise) collision with any obstacle on the path. + * @tparam pathPoints Trajectory approximation + * @param robotId Robot id to ignore (i.e it self) + * @param avoidObjects what objects to avoid + * @param timeOffset Time offset to start checking from + */ + template + [[nodiscard]] std::optional getFirstCollision(std::span path, int robotId, const stp::AvoidObjects& avoidObjects, int timeOffset = 0) const; /** - * Check if the line intersects the defense area (adding a margin equal to the robot collision radius) - * and return the closest point of intersection - * @param point first point of the line - * @param nextPoint second point of the line - * @return the closest intersection with the defense area, or std::nullopt if there is no intersection + * @brief Check if given position is occupied. + * Occupied means that there is and object in the given position that is *not* moving. + * @tparam position Position to check. + * @param robotId Robot id to ignore (i.e it self) + * @param avoidObjects what objects to avoid */ - std::optional getDefenseAreaCollision(const Vector2& point, const Vector2& nextPoint); + template + [[nodiscard]] bool isOccupied(T& position, int robotId, const stp::AvoidObjects& avoidObjects) const; /** - * Calls the defense area collision and robot collision and returns the closest one to the first point - * @param point first point of the line - * @param nextPoint second point of the line - * @return the closest collision point with a robot / the defense area; std::nullopt if no collisions + * @brief Set minimal allowed distance from the ball. + * @tparam distance minimal allowed distance. */ - std::optional getCollisionBetweenPoints(const Vector2& point, const Vector2& nextPoint); + void setMinBallDistance(double distance); - std::vector getRobotPositions(); + /** + * @brief Updates filed reference for collision checking with playing field and defense area. + * @param field Playing field. + */ + void setField(const std::optional& field); - void setField(const rtt::world::Field& field); + /** + * @brief Updates the timeline with obstacles positions. Must be called *once* at the start of each tick. + * For enemy robots and the ball the position is approximated. + * For our robots only the observer position is updated (i.e the current position) or + * if our robot is not moving the current position is set for all time steps. + * @param robots Robots to update the timeline with + * @param ball Ball to update the timeline with + */ + void updateTimeline(const std::vector& robots, const std::optional& ball); - void setRobotPositions(std::vector& robotPositions); + /** + * @brief Updates the timeline with path approximation generated by path planning. + * @param path Trajectory approximation. + * @param currentPosition Current position of the robot. + * @param robotId Id of robot to update the path for + */ + void updateTimelineForOurRobot(std::span path, const Vector2& currentPosition, int robotId); }; - } // namespace rtt::ai::control -#endif // RTT_COLLISIONDETECTOR_H diff --git a/include/roboteam_ai/control/positionControl/PathPointNode.h b/include/roboteam_ai/control/positionControl/PathPointNode.h deleted file mode 100644 index cafba6d3ed..0000000000 --- a/include/roboteam_ai/control/positionControl/PathPointNode.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by ratoone on 20-02-20. -// - -#ifndef RTT_PATHPOINTNODE_H -#define RTT_PATHPOINTNODE_H - -#include - -namespace rtt::ai::control { -/** - * A node in the path tree generation. Has a position and a parent - */ -class PathPointNode { - private: - Vector2 position; - PathPointNode *parent = nullptr; - - public: - /** - * Create a new tree node with a null parent (this will usually be the root of the tree) - * @param position the position represented by the node - */ - explicit PathPointNode(const Vector2 &position); - - /** - * Create a new tree node with the specified parent - * @param position the position represented by the node - * @param parent the parent node of the current node - */ - PathPointNode(const Vector2 &position, PathPointNode &parent); - - [[nodiscard]] const Vector2 &getPosition() const; - - [[nodiscard]] PathPointNode *getParent() const; - - void setParent(PathPointNode &parent); -}; -} // namespace rtt::ai::control - -#endif // RTT_PATHPOINTNODE_H diff --git a/include/roboteam_ai/control/positionControl/PositionControl.h b/include/roboteam_ai/control/positionControl/PositionControl.h index 6f5d9b525e..03e04b96d8 100644 --- a/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/include/roboteam_ai/control/positionControl/PositionControl.h @@ -5,89 +5,33 @@ #ifndef RTT_POSITIONCONTROL_H #define RTT_POSITIONCONTROL_H +#include #include -#include "BBTrajectories/Trajectory2D.h" -#include "CollisionDetector.h" -#include "control/positionControl/BBTrajectories/WorldObjects.h" -#include "control/positionControl/pathPlanning/NumTreesPlanning.h" +#include "control/positionControl/CollisionDetector.h" +#include "control/positionControl/pathPlanning/BBTPathPlanning.h" #include "control/positionControl/pathTracking/BBTPathTracking.h" -#include "control/positionControl/pathTracking/DensePathTracking.h" -#include "control/positionControl/pathTracking/PidTracking.h" #include "utilities/StpInfoEnums.h" namespace rtt::ai::control { +struct PositionControlCommand { + RobotCommand robotCommand; + bool isOccupied = false; +}; + /** * The main position control class. Use this for your robot position control * requirements. */ class PositionControl { private: - /// the distance to the target position at which the robot will stop if it - /// detects a collision (e.g. target is inside a robot) - static constexpr double FINAL_AVOIDANCE_DISTANCE = 4 * Constants::ROBOT_RADIUS(); - CollisionDetector collisionDetector; - rtt::BB::WorldObjects worldObjects; - NumTreesPlanning pathPlanningAlgorithm = NumTreesPlanning(collisionDetector); - DensePathTracking pathTrackingAlgorithm; - BBTPathTracking pathTrackingAlgorithmBBT; - - std::unordered_map computedTrajectories; - std::unordered_map> computedPaths; - std::unordered_map> computedPathsVel; - std::unordered_map>> computedPathsPosVel; - - /** - * @brief Checks if the current path should be recalculated: - * @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 currentPosition the current position of the aforementioned robot - * @param currentVelocity its velocity - * @param targetPosition the desired position that the robot has to reachsho - * @return Boolean that is 1 if the path needs to be recalculated - */ - bool shouldRecalculateTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 targetPosition, ai::stp::AvoidObjects); + std::unordered_map> pathControllers; public: /** - * Generates a path according to the selected planning algorithm, - * and tracks it using the selected tracking algorithm. In the case a collision - * is detected (using the collision detector), the path is recalculated. - * @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 currentPosition the current position of the aforementioned robot - * @param currentVelocity its velocity - * @param targetPosition the desired position that the robot has to reach - * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @return a RobotCommand, which can be fed directly in the output - */ - RobotCommand computeAndTrackPath(const rtt_world::Field &field, int robotId, const Vector2 ¤tPosition, const Vector2 ¤tVelocity, Vector2 &targetPosition, - stp::PIDType pidType); - - /** - * Updates the robot view vector - * @param robotPositions the position vector of the robots - */ - void setRobotPositions(std::vector &robotPositions); - - /** - * The computed path should be recalculated if:
- * - it is empty (no path yet)
- * - the target position changed with at least MAX_TARGET_DEVIATION
- * - the robot will collide with another one by the next path point (ignored if the robot is not moving) - * @param targetPos final target position - * @param robotId the ID of the current robot - * @return true if one of the above conditions are true, false otherwise - */ - bool shouldRecalculatePath(const Vector2 ¤tPosition, const Vector2 &targetPos, const Vector2 ¤tVelocity, int robotId); - - /** - * @brief Generates a collision-free trajectory and tracks it. Returns also possibly - * the location of a collision on the path if no correct path can be found - * @param world a pointer to the current world + * @brief Generates a collision-free trajectory and tracks it. * @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 currentPosition the current position of the aforementioned robot @@ -95,70 +39,16 @@ class PositionControl { * @param targetPosition the desired position that the robot has to reach * @param maxRobotVelocity the maximum velocity that the robot is allowed to have * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @return A RobotCommand and optional with the location of the first collision on the path - */ - rtt::BB::CommandCollision computeAndTrackTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 currentPosition, - Vector2 currentVelocity, Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, - stp::AvoidObjects avoidObjects); - - /** - * @brief Tries to find a new trajectory when the current path has a collision on it. It tries this by - * looking for trajectories which go to intermediate points in the area of the collision and from these - * paths again to the target. Also draws the intermediate point and path in the interface - * @param world the world object - * @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 currentPosition the current position of the aforementioned robot - * @param currentVelocity its velocity - * @param firstCollision location of the first collision on the current path - * @param targetPosition the desired position that the robot has to reach - * @param maxRobotVelocity the maximum velocity the robot is allowed to have - * @param timeStep the time between path points when approaching the path - * @return An optional with a new path - */ - std::optional findNewTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 ¤tPosition, Vector2 ¤tVelocity, - std::optional &firstCollision, Vector2 &targetPosition, double maxRobotVelocity, double timeStep, - stp::AvoidObjects AvoidObjects); - - // If no collision on trajectory to intermediatePoint, create new points along this trajectory in timeStep increments. - // Then loop through these new points, generate trajectories from these to the original target and check for collisions. - // Return the first trajectory without collisions, or pop() this point and start checking the next one. - /** - * @brief Calculates a path to the targetPosition from a point on the path to an intermediate path and - * return it if there are no collisions - * @param world the world object - * @param field the field object, used onwards by the collision detector - * @param intermediatePathCollision if intermediatePathCollision has no value, return {} - * @param pathToIntermediatePoint used for getting new start points of the BBT to the targetPosition - * @param targetPosition the desired position that the robot has to reach - * @param robotId the ID of the robot for which the path is calculated - * @param timeStep time in seconds between new start points on the BBT to the intermediatePoint - * @return optional Trajectory if a new path was found - */ - std::optional calculateTrajectoryAroundCollision(const rtt::world::World *world, const rtt::world::Field &field, - std::optional &intermediatePathCollision, Trajectory2D trajectoryToIntermediatePoint, - Vector2 &targetPosition, int robotId, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects); - - /** - * Creates intermediate points to make a path to. First, a pointToDrawFrom is picked by drawing a line - * from the target position to the obstacle and extending that line further towards our currentPosition. - * Second, make half circle of intermediatePoints pointed towards obstaclePosition, originating from pointToDrawFrom - * @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 targetPosition the desired position that the robot has to reach - * @return A vector with coordinates of the intermediate points + * @param avoidObjects The objects that the robot should avoid + * @return A PositionControlCommand containing the robot command and path flags */ - std::vector createIntermediatePoints(const rtt::world::Field &field, int robotId, std::optional &firstCollision, Vector2 &targetPosition); + PositionControlCommand computeAndTrackTrajectory(const rtt::world::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, Vector2 targetPosition, + double maxRobotVelocity, stp::PIDType pidType, stp::AvoidObjects avoidObjects); /** - * @brief Gives each intermediate point a score for how close the point is to the collisionPosition - * @param intermediatePoints the intermediate points for trying to find a new path - * @param firstCollision used for scoring the points - * @return A priority_queue to sort the points + * @brief Returns reference to the collision detector. */ - std::priority_queue, std::vector>, std::greater<>> scoreIntermediatePoints( - std::vector &intermediatePoints, std::optional &firstCollision); + CollisionDetector &getCollisionDetector(); }; } // namespace rtt::ai::control diff --git a/include/roboteam_ai/control/positionControl/PositionControlUtils.h b/include/roboteam_ai/control/positionControl/PositionControlUtils.h index f9412935af..b9ddef46f9 100644 --- a/include/roboteam_ai/control/positionControl/PositionControlUtils.h +++ b/include/roboteam_ai/control/positionControl/PositionControlUtils.h @@ -5,18 +5,13 @@ #ifndef RTT_POSITIONCONTROLUTILS_H #define RTT_POSITIONCONTROLUTILS_H +#include "interface/api/Output.h" #include "roboteam_utils/Vector2.h" -#include "utilities/Constants.h" +#include "utilities/StpInfoEnums.h" namespace rtt::ai::control { class PositionControlUtils { - private: - static constexpr double MAX_TARGET_DEVIATION = 0.05; - - // minimum distance needed to consider the current target reached - static constexpr double MIN_DISTANCE_TARGET_REACHED = 2 * Constants::ROBOT_RADIUS(); - public: /** * If the distance between the old target and the new target > MAX_TARGET_DEVIATION @@ -35,11 +30,25 @@ class PositionControlUtils { static bool isTargetReached(const Vector2 &targetPos, const Vector2 ¤tPosition); /** - * Removes the first element in the path in the case the first point is reached - * @param path the vector of path points - * @param currentPosition the current position of the robot + * Is the target moving + * @param velocity */ - static void removeFirstIfReached(std::vector &path, const Vector2 ¤tPosition); + static bool isMoving(const Vector2 &velocity); + + static pidVals getPIDValue(const stp::PIDType &pidType) { + switch (pidType) { + case stp::PIDType::DEFAULT: + return interface::Output::getNumTreePid(); + case stp::PIDType::RECEIVE: + return interface::Output::getReceivePid(); + case stp::PIDType::INTERCEPT: + return interface::Output::getInterceptPid(); + case stp::PIDType::KEEPER: + return interface::Output::getKeeperPid(); + case stp::PIDType::KEEPER_INTERCEPT: + return interface::Output::getKeeperInterceptPid(); + } + } }; } // namespace rtt::ai::control diff --git a/include/roboteam_ai/control/positionControl/pathPlanning/BBTPathPlanning.h b/include/roboteam_ai/control/positionControl/pathPlanning/BBTPathPlanning.h new file mode 100644 index 0000000000..629d006911 --- /dev/null +++ b/include/roboteam_ai/control/positionControl/pathPlanning/BBTPathPlanning.h @@ -0,0 +1,92 @@ +// +// Created by martin on 14-5-22. +// + +#pragma once + +#include + +#include "control/positionControl/CollisionDetector.h" +namespace rtt::ai::control { + +/** + * \brief Wrapper container for 2 trajectories and path-score + * Combining 2 trajectories is an _expensive_ operation, thus it's better to defer that step to the latest possible moment. + * cutoffIndex indicates where endTrajectory connects with initialTrajectory (i.e how many points are used from the initialTrajectory) + * Lower score indicated better path (e.g score = 100 is better than score = 150) + */ +struct CombinedTrajectory { + BB::BBTrajectory2D initialTrajectory; + BB::BBTrajectory2D endTrajectory; + int cutoffIndex = 0; + int score = std::numeric_limits::max(); +}; + +/** + * Path planning using the BBT algorithm for single robot. + */ +class BBTPathPlanning { + private: + static constexpr int INTERMEDIATE_POINTS_PER_CIRCLE = 8; + static constexpr double ANGLE_BETWEEN_INTERMEDIATE_POINTS = 2 * M_PI / INTERMEDIATE_POINTS_PER_CIRCLE; + + const int robotId; + const CollisionDetector& collisionDetector; + + double fieldWidth; + double maxVelocity; + stp::AvoidObjects avoidObjects; + + /** + * \brief Calculates the score of given trajectory. Lower score indicated better path (e.g score = 100 is better than score = 150) + * @param duration How long the path is (i.e. how many path segments there are in the final path) + * @param collision information about *first* collision on the path + */ + [[nodiscard]] static int scorePath(int duration, std::optional collision); + + /** + * \brief Find the best trajectory from initial position to the target position that is skewed towards the intermediatePoint. + * 1) Trajectory is generated from initialPos to intermediatePoint. + * 2) Trajectory is generated from each path point to the target position. + * 3) Best trajectory is selected. + * @param initialPos Initial Position + * @param initialVel Initial Velocity + * @param intermediatePoint Is used to generate intermediate trajectory. + * @param targetPos Target Position + */ + [[nodiscard]] CombinedTrajectory bestTrajectoryForIntermediatePoint(const Vector2& initialPos, const Vector2& initialVel, const Vector2& intermediatePoint, + const Vector2& targetPos) const; + + /** + * \brief Generates vector of intermediate points + * @param center Intermediate points are generated around this point + */ + [[nodiscard]] std::vector generateIntermediatePoints(const Vector2& center) const; + + /** + * \brief Combines two BBTrajectories2D into one PosVelVector vector + * @param combineTraj Trajectories to combine + */ + [[nodiscard]] std::vector extractPath(CombinedTrajectory&& combineTraj) const; + + public: + BBTPathPlanning(double fieldWidth, double maxVelocity, int robotId, const CollisionDetector& collisionDetector); + + /** + * \brief Computes best trajectory to reach the target position + * @param initialPos Initial Position + * @param initialVel Initial Velocity + * @param targetPos Target Position + */ + [[nodiscard]] std::vector generateNewPath(const Vector2& initialPos, const Vector2& initialVel, const Vector2& targetPos) const; + + /** + * \brief Updates the path planning with new information about the world + * @param field Playing filed + * @param newAvoidObjects New information about objects to avoid + * @param newMaxVelocity New maximum velocity of the robot + */ + void updateConstraints(const rtt::world::Field& field, stp::AvoidObjects newAvoidObjects, double newMaxVelocity); +}; + +} // namespace rtt::ai::control diff --git a/include/roboteam_ai/control/positionControl/pathPlanning/NumTreesPlanning.h b/include/roboteam_ai/control/positionControl/pathPlanning/NumTreesPlanning.h deleted file mode 100644 index 07da1fdf83..0000000000 --- a/include/roboteam_ai/control/positionControl/pathPlanning/NumTreesPlanning.h +++ /dev/null @@ -1,57 +0,0 @@ -// -// Created by ratoone on 20-02-20. -// - -#ifndef RTT_NUMTREESPLANNING_H -#define RTT_NUMTREESPLANNING_H - -#include - -#include "PathPlanningAlgorithm.h" -#include "control/positionControl/CollisionDetector.h" -#include "control/positionControl/PathPointNode.h" -#include "utilities/Constants.h" - -namespace rtt::ai::control { - -class NumTreesPlanning : public PathPlanningAlgorithm { - private: - static constexpr double AVOIDANCE_DISTANCE = 5 * Constants::ROBOT_RADIUS(); - static constexpr double TARGET_THRESHOLD = 0.1; - static constexpr int MAX_BRANCHING = 10; - static constexpr int MAX_ITERATIONS = 10; - - /** - * Generate 2 new points to the side of the collisionPosition, such that the points and the parent point form - * an isosceles triangle, with the collisionPosition being the middle of the base. The first point will bt the one - * closest to the destimation. - * @param parentPoint starting point - * @param collisionPosition the point to branch from - * @param destination the target position. - * @return - */ - std::vector branchPath(PathPointNode &parentPoint, const Vector2 &collisionPosition, Vector2 &destination) const; - - public: - /** - * The collision detector is provided by the position control. This class was intended - * to be used only with the PositionControl - * @param collisionDetector - */ - explicit NumTreesPlanning(CollisionDetector &collisionDetector); - - /** - * Computes a path using the implemented algorithm. It takes into account the - * obstacles present in the field.

- * NumTreesPlanning uses an algorithm wrote by an old team member. It tries to - * trace a path to the destination, and if there is a collision, it traces back - * and branches into two points to the side of the obstacle, trying them afterwards. - * @param robotPosition the current robot position - * @param targetPosition the goal position - * @return a list of points representing the path - */ - std::vector computePath(const Vector2 &robotPosition, Vector2 &targetPosition) override; -}; -} // namespace rtt::ai::control - -#endif // RTT_NUMTREESPLANNING_H diff --git a/include/roboteam_ai/control/positionControl/pathPlanning/PathPlanningAlgorithm.h b/include/roboteam_ai/control/positionControl/pathPlanning/PathPlanningAlgorithm.h deleted file mode 100644 index 885b2ddaef..0000000000 --- a/include/roboteam_ai/control/positionControl/pathPlanning/PathPlanningAlgorithm.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by ratoone on 09-03-20. -// - -#ifndef RTT_PATHPLANNINGALGORITHM_H -#define RTT_PATHPLANNINGALGORITHM_H - -#include "control/positionControl/CollisionDetector.h" - -namespace rtt::ai::control { -/** - * The base class for the path planning algorithms. All future algorithms should inherit - * from this - */ -class PathPlanningAlgorithm { - protected: - CollisionDetector &collisionDetector; - - public: - /** - * The collision detector is provided by the position control. This class was intended - * to be used only with the PositionControl - * @param collisionDetector - */ - explicit PathPlanningAlgorithm(CollisionDetector &collisionDetector); - - /** - * Algorithm specific path computation. It should take into account the obstacles in the field - * @param robotPosition the current robot position - * @param targetPosition the goal position - * @return a list of points representing the path - */ - virtual std::vector computePath(const Vector2 &robotPosition, Vector2 &targetPosition) = 0; -}; -} // namespace rtt::ai::control - -#endif // RTT_PATHPLANNINGALGORITHM_H diff --git a/include/roboteam_ai/control/positionControl/pathPlanning/Voronoi.h b/include/roboteam_ai/control/positionControl/pathPlanning/Voronoi.h deleted file mode 100644 index b255626c06..0000000000 --- a/include/roboteam_ai/control/positionControl/pathPlanning/Voronoi.h +++ /dev/null @@ -1,1372 +0,0 @@ -// Copyright (c) 2015-2019 Mathias Westerdahl -// For LICENSE (MIT), USAGE or HISTORY, see bottom of file -/** - * Implementation of the Voronoi diagram generation based on Fortune's - * algorithm. Header-only library taken from https://github.com/JCash/voronoi - * Two changes were made from the initial file: - * - added #define implementation (line 14) - */ - -#ifndef JC_VORONOI_H -#define JC_VORONOI_H - -#define JC_VORONOI_IMPLEMENTATION - -#include -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef JCV_REAL_TYPE -#define JCV_REAL_TYPE float -#endif - -#ifndef JCV_ATAN2 -#define JCV_ATAN2(_Y_, _X_) atan2f(_Y_, _X_) -#endif - -#ifndef JCV_SQRT -#define JCV_SQRT(_X_) sqrtf(_X_) -#endif - -#ifndef JCV_PI -#define JCV_PI 3.14159265358979323846264338327950288f -#endif - -#ifndef JCV_FLT_MAX -#define JCV_FLT_MAX 3.402823466e+38F -#endif - -#ifndef JCV_EDGE_INTERSECT_THRESHOLD -// Fix for Issue #40 -#define JCV_EDGE_INTERSECT_THRESHOLD 1.0e-10F -#endif - -typedef JCV_REAL_TYPE jcv_real; - -typedef struct _jcv_point jcv_point; -typedef struct _jcv_rect jcv_rect; -typedef struct _jcv_site jcv_site; -typedef struct _jcv_edge jcv_edge; -typedef struct _jcv_graphedge jcv_graphedge; -typedef struct _jcv_diagram jcv_diagram; -typedef struct _jcv_clipper jcv_clipper; -typedef struct _jcv_context_internal jcv_context_internal; - -/// Tests if a point is inside the final shape -typedef int (*jcv_clip_test_point_fn)(const jcv_clipper* clipper, const jcv_point p); -/** Given an edge, and the clipper, calculates the e->pos[0] and e->pos[1] - * Returns 0 if not successful - */ -typedef int (*jcv_clip_edge_fn)(const jcv_clipper* clipper, jcv_edge* e); -/** Given the clipper, the site and the last edge, - * closes any gaps in the polygon by adding new edges that follow the bounding shape - * The internal context is use when allocating new edges. - */ -typedef void (*jcv_clip_fillgap_fn)(const jcv_clipper* clipper, jcv_context_internal* allocator, jcv_site* s); - -/** - * Uses malloc - * If a clipper is not supplied, a default box clipper will be used - * If rect is null, an automatic bounding box is calculated, with an extra padding of 10 units - * All points will be culled against the bounding rect, and all edges will be clipped against it. - */ -extern void jcv_diagram_generate(int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, jcv_diagram* diagram); - -typedef void* (*FJCVAllocFn)(void* userctx, size_t size); -typedef void (*FJCVFreeFn)(void* userctx, void* p); - -// Same as above, but allows the client to use a custom allocator -extern void jcv_diagram_generate_useralloc(int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, void* userallocctx, FJCVAllocFn allocfn, - FJCVFreeFn freefn, jcv_diagram* diagram); - -// Uses free (or the registered custom free function) -extern void jcv_diagram_free(jcv_diagram* diagram); - -// Returns an array of sites, where each index is the same as the original input point array. -extern const jcv_site* jcv_diagram_get_sites(const jcv_diagram* diagram); - -// Returns a linked list of all the voronoi edges -// excluding the ones that lie on the borders of the bounding box. -// For a full list of edges, you need to iterate over the sites, and their graph edges. -extern const jcv_edge* jcv_diagram_get_edges(const jcv_diagram* diagram); - -// Iterates over a list of edges, skipping invalid edges (where p0==p1) -extern const jcv_edge* jcv_diagram_get_next_edge(const jcv_edge* edge); - -// For the default clipper -extern int jcv_boxshape_test(const jcv_clipper* clipper, const jcv_point p); -extern int jcv_boxshape_clip(const jcv_clipper* clipper, jcv_edge* e); -extern void jcv_boxshape_fillgaps(const jcv_clipper* clipper, jcv_context_internal* allocator, jcv_site* s); - -#pragma pack(push, 1) - -struct _jcv_point { - jcv_real x; - jcv_real y; -}; - -struct _jcv_graphedge { - struct _jcv_graphedge* next; - struct _jcv_edge* edge; - struct _jcv_site* neighbor; - jcv_point pos[2]; - jcv_real angle; -}; - -struct _jcv_site { - jcv_point p; - int index; // Index into the original list of points - jcv_graphedge* edges; // The half edges owned by the cell -}; - -// The coefficients a, b and c are from the general line equation: ax * by + c = 0 -struct _jcv_edge { - struct _jcv_edge* next; - jcv_site* sites[2]; - jcv_point pos[2]; - jcv_real a; - jcv_real b; - jcv_real c; -}; - -struct _jcv_rect { - jcv_point min; - jcv_point max; -}; - -struct _jcv_clipper { - jcv_clip_test_point_fn test_fn; - jcv_clip_edge_fn clip_fn; - jcv_clip_fillgap_fn fill_fn; - jcv_point min; // The bounding rect min - jcv_point max; // The bounding rect max - void* ctx; // User defined context -}; - -struct _jcv_diagram { - jcv_context_internal* internal; - jcv_edge* edges; - jcv_site* sites; - int numsites; - jcv_point min; - jcv_point max; -}; - -#pragma pack(pop) - -#ifdef __cplusplus -} -#endif - -#endif // JC_VORONOI_H - -#ifdef JC_VORONOI_IMPLEMENTATION -#undef JC_VORONOI_IMPLEMENTATION - -#include - -// INTERNAL FUNCTIONS - -#if defined(_MSC_VER) && !defined(__cplusplus) -#define inline __inline -#endif - -// jcv_point - -static inline int jcv_point_cmp(const void* p1, const void* p2) { - const jcv_point* s1 = (const jcv_point*)p1; - const jcv_point* s2 = (const jcv_point*)p2; - return (s1->y != s2->y) ? (s1->y < s2->y ? -1 : 1) : (s1->x < s2->x ? -1 : 1); -} - -static inline int jcv_point_less(const jcv_point* pt1, const jcv_point* pt2) { return (pt1->y == pt2->y) ? (pt1->x < pt2->x) : pt1->y < pt2->y; } - -static inline int jcv_point_eq(const jcv_point* pt1, const jcv_point* pt2) { return (pt1->y == pt2->y) && (pt1->x == pt2->x); } - -static inline int jcv_point_on_box_edge(const jcv_point* pt, const jcv_point* min, const jcv_point* max) { - return pt->x == min->x || pt->y == min->y || pt->x == max->x || pt->y == max->y; -} - -static inline jcv_real jcv_point_dist_sq(const jcv_point* pt1, const jcv_point* pt2) { - jcv_real diffx = pt1->x - pt2->x; - jcv_real diffy = pt1->y - pt2->y; - return diffx * diffx + diffy * diffy; -} - -static inline jcv_real jcv_point_dist(const jcv_point* pt1, const jcv_point* pt2) { return (jcv_real)(JCV_SQRT(jcv_point_dist_sq(pt1, pt2))); } - -// Structs - -#pragma pack(push, 1) - -typedef struct _jcv_halfedge { - jcv_edge* edge; - struct _jcv_halfedge* left; - struct _jcv_halfedge* right; - jcv_point vertex; - jcv_real y; - int direction; // 0=left, 1=right - int pqpos; -} jcv_halfedge; - -typedef struct _jcv_memoryblock { - size_t sizefree; - struct _jcv_memoryblock* next; - char* memory; -} jcv_memoryblock; - -typedef int (*FJCVPriorityQueuePrint)(const void* node, int pos); - -typedef struct _jcv_priorityqueue { - // Implements a binary heap - int maxnumitems; - int numitems; - void** items; -} jcv_priorityqueue; - -struct _jcv_context_internal { - void* mem; - jcv_edge* edges; - jcv_halfedge* beachline_start; - jcv_halfedge* beachline_end; - jcv_halfedge* last_inserted; - jcv_priorityqueue* eventqueue; - - jcv_site* sites; - jcv_site* bottomsite; - int numsites; - int currentsite; - int _padding; - - jcv_memoryblock* memblocks; - jcv_edge* edgepool; - jcv_halfedge* halfedgepool; - void** eventmem; - jcv_clipper clipper; - - void* memctx; // Given by the user - FJCVAllocFn alloc; - FJCVFreeFn free; - - jcv_rect rect; -}; - -#pragma pack(pop) - -static const int JCV_DIRECTION_LEFT = 0; -static const int JCV_DIRECTION_RIGHT = 1; -static const jcv_real JCV_INVALID_VALUE = (jcv_real)-JCV_FLT_MAX; - -void inline jcv_diagram_free(jcv_diagram* d) { - jcv_context_internal* internal = d->internal; - void* memctx = internal->memctx; - FJCVFreeFn freefn = internal->free; - while (internal->memblocks) { - jcv_memoryblock* p = internal->memblocks; - internal->memblocks = internal->memblocks->next; - freefn(memctx, p); - } - - freefn(memctx, internal->mem); -} - -const inline jcv_site* jcv_diagram_get_sites(const jcv_diagram* diagram) { return diagram->internal->sites; } - -const inline jcv_edge* jcv_diagram_get_edges(const jcv_diagram* diagram) { - jcv_edge e; - e.next = diagram->internal->edges; - return jcv_diagram_get_next_edge(&e); -} - -const inline jcv_edge* jcv_diagram_get_next_edge(const jcv_edge* edge) { - const jcv_edge* e = edge->next; - while (e != 0 && jcv_point_eq(&e->pos[0], &e->pos[1])) { - e = e->next; - } - return e; -} - -static inline void* jcv_alloc(jcv_context_internal* internal, size_t size) { - if (!internal->memblocks || internal->memblocks->sizefree < size) { - size_t blocksize = 16 * 1024; - jcv_memoryblock* block = (jcv_memoryblock*)internal->alloc(internal->memctx, blocksize); - size_t offset = sizeof(jcv_memoryblock); - block->sizefree = blocksize - offset; - block->next = internal->memblocks; - block->memory = ((char*)block) + offset; - internal->memblocks = block; - } - void* p = internal->memblocks->memory; - internal->memblocks->memory += size; - internal->memblocks->sizefree -= size; - return p; -} - -static inline jcv_edge* jcv_alloc_edge(jcv_context_internal* internal) { return (jcv_edge*)jcv_alloc(internal, sizeof(jcv_edge)); } - -static inline jcv_halfedge* jcv_alloc_halfedge(jcv_context_internal* internal) { - if (internal->halfedgepool) { - jcv_halfedge* edge = internal->halfedgepool; - internal->halfedgepool = internal->halfedgepool->right; - return edge; - } - - return (jcv_halfedge*)jcv_alloc(internal, sizeof(jcv_halfedge)); -} - -static inline jcv_graphedge* jcv_alloc_graphedge(jcv_context_internal* internal) { return (jcv_graphedge*)jcv_alloc(internal, sizeof(jcv_graphedge)); } - -static inline void* jcv_alloc_fn(void* memctx, size_t size) { - (void)memctx; - return malloc(size); -} - -static inline void jcv_free_fn(void* memctx, void* p) { - (void)memctx; - free(p); -} - -// jcv_edge - -static inline int jcv_is_valid(const jcv_point* p) { return (p->x != JCV_INVALID_VALUE || p->y != JCV_INVALID_VALUE) ? 1 : 0; } - -static void jcv_edge_create(jcv_edge* e, jcv_site* s1, jcv_site* s2) { - e->next = 0; - e->sites[0] = s1; - e->sites[1] = s2; - e->pos[0].x = JCV_INVALID_VALUE; - e->pos[0].y = JCV_INVALID_VALUE; - e->pos[1].x = JCV_INVALID_VALUE; - e->pos[1].y = JCV_INVALID_VALUE; - - // Create line equation between S1 and S2: - // jcv_real a = -1 * (s2->p.y - s1->p.y); - // jcv_real b = s2->p.x - s1->p.x; - // //jcv_real c = -1 * (s2->p.x - s1->p.x) * s1->p.y + (s2->p.y - s1->p.y) * s1->p.x; - // - // // create perpendicular line - // jcv_real pa = b; - // jcv_real pb = -a; - // //jcv_real pc = pa * s1->p.x + pb * s1->p.y; - // - // // Move to the mid point - // jcv_real mx = s1->p.x + dx * jcv_real(0.5); - // jcv_real my = s1->p.y + dy * jcv_real(0.5); - // jcv_real pc = ( pa * mx + pb * my ); - - jcv_real dx = s2->p.x - s1->p.x; - jcv_real dy = s2->p.y - s1->p.y; - int dx_is_larger = (dx * dx) > (dy * dy); // instead of fabs - - // Simplify it, using dx and dy - e->c = dx * (s1->p.x + dx * (jcv_real)0.5) + dy * (s1->p.y + dy * (jcv_real)0.5); - - if (dx_is_larger) { - e->a = (jcv_real)1; - e->b = dy / dx; - e->c /= dx; - } else { - e->a = dx / dy; - e->b = (jcv_real)1; - e->c /= dy; - } -} - -// CLIPPING -int inline jcv_boxshape_test(const jcv_clipper* clipper, const jcv_point p) { - return p.x >= clipper->min.x && p.x <= clipper->max.x && p.y >= clipper->min.y && p.y <= clipper->max.y; -} - -// The line equation: ax + by + c = 0 -// see jcv_edge_create -int inline jcv_boxshape_clip(const jcv_clipper* clipper, jcv_edge* e) { - jcv_real pxmin = clipper->min.x; - jcv_real pxmax = clipper->max.x; - jcv_real pymin = clipper->min.y; - jcv_real pymax = clipper->max.y; - - jcv_real x1, y1, x2, y2; - jcv_point* s1; - jcv_point* s2; - if (e->a == (jcv_real)1 && e->b >= (jcv_real)0) { - s1 = jcv_is_valid(&e->pos[1]) ? &e->pos[1] : 0; - s2 = jcv_is_valid(&e->pos[0]) ? &e->pos[0] : 0; - } else { - s1 = jcv_is_valid(&e->pos[0]) ? &e->pos[0] : 0; - s2 = jcv_is_valid(&e->pos[1]) ? &e->pos[1] : 0; - } - - if (e->a == (jcv_real)1) // delta x is larger - { - y1 = pymin; - if (s1 != 0 && s1->y > pymin) { - y1 = s1->y; - } - if (y1 > pymax) { - y1 = pymax; - } - x1 = e->c - e->b * y1; - y2 = pymax; - if (s2 != 0 && s2->y < pymax) y2 = s2->y; - - if (y2 < pymin) { - y2 = pymin; - } - x2 = (e->c) - (e->b) * y2; - // Never occurs according to lcov - // if( ((x1 > pxmax) & (x2 > pxmax)) | ((x1 < pxmin) & (x2 < pxmin)) ) - // { - // return 0; - // } - if (x1 > pxmax) { - x1 = pxmax; - y1 = (e->c - x1) / e->b; - } else if (x1 < pxmin) { - x1 = pxmin; - y1 = (e->c - x1) / e->b; - } - if (x2 > pxmax) { - x2 = pxmax; - y2 = (e->c - x2) / e->b; - } else if (x2 < pxmin) { - x2 = pxmin; - y2 = (e->c - x2) / e->b; - } - } else // delta y is larger - { - x1 = pxmin; - if (s1 != 0 && s1->x > pxmin) x1 = s1->x; - if (x1 > pxmax) { - x1 = pxmax; - } - y1 = e->c - e->a * x1; - x2 = pxmax; - if (s2 != 0 && s2->x < pxmax) x2 = s2->x; - if (x2 < pxmin) { - x2 = pxmin; - } - y2 = e->c - e->a * x2; - // Never occurs according to lcov - // if( ((y1 > pymax) & (y2 > pymax)) | ((y1 < pymin) & (y2 < pymin)) ) - // { - // return 0; - // } - if (y1 > pymax) { - y1 = pymax; - x1 = (e->c - y1) / e->a; - } else if (y1 < pymin) { - y1 = pymin; - x1 = (e->c - y1) / e->a; - } - if (y2 > pymax) { - y2 = pymax; - x2 = (e->c - y2) / e->a; - } else if (y2 < pymin) { - y2 = pymin; - x2 = (e->c - y2) / e->a; - } - } - - e->pos[0].x = x1; - e->pos[0].y = y1; - e->pos[1].x = x2; - e->pos[1].y = y2; - - // If the two points are equal, the result is invalid - return (x1 == x2 && y1 == y2) ? 0 : 1; -} - -// The line equation: ax + by + c = 0 -// see jcv_edge_create -static int jcv_edge_clipline(jcv_context_internal* internal, jcv_edge* e) { return internal->clipper.clip_fn(&internal->clipper, e); } - -static jcv_edge* jcv_edge_new(jcv_context_internal* internal, jcv_site* s1, jcv_site* s2) { - jcv_edge* e = jcv_alloc_edge(internal); - jcv_edge_create(e, s1, s2); - return e; -} - -// jcv_halfedge - -static void jcv_halfedge_link(jcv_halfedge* edge, jcv_halfedge* newedge) { - newedge->left = edge; - newedge->right = edge->right; - edge->right->left = newedge; - edge->right = newedge; -} - -static inline void jcv_halfedge_unlink(jcv_halfedge* he) { - he->left->right = he->right; - he->right->left = he->left; - he->left = 0; - he->right = 0; -} - -static inline jcv_halfedge* jcv_halfedge_new(jcv_context_internal* internal, jcv_edge* e, int direction) { - jcv_halfedge* he = jcv_alloc_halfedge(internal); - he->edge = e; - he->left = 0; - he->right = 0; - he->direction = direction; - he->pqpos = 0; - // These are set outside - // he->y - // he->vertex - return he; -} - -static void jcv_halfedge_delete(jcv_context_internal* internal, jcv_halfedge* he) { - he->right = internal->halfedgepool; - internal->halfedgepool = he; -} - -static inline jcv_site* jcv_halfedge_leftsite(const jcv_halfedge* he) { return he->edge->sites[he->direction]; } - -static inline jcv_site* jcv_halfedge_rightsite(const jcv_halfedge* he) { return he->edge ? he->edge->sites[1 - he->direction] : 0; } - -static int jcv_halfedge_rightof(const jcv_halfedge* he, const jcv_point* p) { - const jcv_edge* e = he->edge; - const jcv_site* topsite = e->sites[1]; - - int right_of_site = (p->x > topsite->p.x) ? 1 : 0; - if (right_of_site && he->direction == JCV_DIRECTION_LEFT) return 1; - if (!right_of_site && he->direction == JCV_DIRECTION_RIGHT) return 0; - - jcv_real dxp, dyp, dxs, t1, t2, t3, yl; - - int above; - if (e->a == (jcv_real)1) { - dyp = p->y - topsite->p.y; - dxp = p->x - topsite->p.x; - int fast = 0; - if ((!right_of_site & (e->b < (jcv_real)0)) | (right_of_site & (e->b >= (jcv_real)0))) { - above = dyp >= e->b * dxp; - fast = above; - } else { - above = (p->x + p->y * e->b) > e->c; - if (e->b < (jcv_real)0) above = !above; - if (!above) fast = 1; - } - if (!fast) { - dxs = topsite->p.x - e->sites[0]->p.x; - above = e->b * (dxp * dxp - dyp * dyp) < dxs * dyp * ((jcv_real)1 + (jcv_real)2 * dxp / dxs + e->b * e->b); - if (e->b < (jcv_real)0) above = !above; - } - } else // e->b == 1 - { - yl = e->c - e->a * p->x; - t1 = p->y - yl; - t2 = p->x - topsite->p.x; - t3 = yl - topsite->p.y; - above = t1 * t1 > (t2 * t2 + t3 * t3); - } - return (he->direction == JCV_DIRECTION_LEFT ? above : !above); -} - -// Keeps the priority queue sorted with events sorted in ascending order -// Return 1 if the edges needs to be swapped -static inline int jcv_halfedge_compare(const jcv_halfedge* he1, const jcv_halfedge* he2) { return (he1->y == he2->y) ? he1->vertex.x > he2->vertex.x : he1->y > he2->y; } - -static int jcv_halfedge_intersect(const jcv_halfedge* he1, const jcv_halfedge* he2, jcv_point* out) { - const jcv_edge* e1 = he1->edge; - const jcv_edge* e2 = he2->edge; - - jcv_real d = e1->a * e2->b - e1->b * e2->a; - if (((jcv_real)-JCV_EDGE_INTERSECT_THRESHOLD < d && d < (jcv_real)JCV_EDGE_INTERSECT_THRESHOLD)) { - return 0; - } - out->x = (e1->c * e2->b - e1->b * e2->c) / d; - out->y = (e1->a * e2->c - e1->c * e2->a) / d; - - const jcv_edge* e; - const jcv_halfedge* he; - if (jcv_point_less(&e1->sites[1]->p, &e2->sites[1]->p)) { - he = he1; - e = e1; - } else { - he = he2; - e = e2; - } - - int right_of_site = out->x >= e->sites[1]->p.x; - if ((right_of_site && he->direction == JCV_DIRECTION_LEFT) || (!right_of_site && he->direction == JCV_DIRECTION_RIGHT)) { - return 0; - } - - return 1; -} - -// Priority queue - -static int jcv_pq_moveup(jcv_priorityqueue* pq, int pos) { - jcv_halfedge** items = (jcv_halfedge**)pq->items; - jcv_halfedge* node = items[pos]; - - for (int parent = (pos >> 1); pos > 1 && jcv_halfedge_compare(items[parent], node); pos = parent, parent = parent >> 1) { - items[pos] = items[parent]; - items[pos]->pqpos = pos; - } - - node->pqpos = pos; - items[pos] = node; - return pos; -} - -static int jcv_pq_maxchild(jcv_priorityqueue* pq, int pos) { - int child = pos << 1; - if (child >= pq->numitems) return 0; - jcv_halfedge** items = (jcv_halfedge**)pq->items; - if ((child + 1) < pq->numitems && jcv_halfedge_compare(items[child], items[child + 1])) return child + 1; - return child; -} - -static int jcv_pq_movedown(jcv_priorityqueue* pq, int pos) { - jcv_halfedge** items = (jcv_halfedge**)pq->items; - jcv_halfedge* node = items[pos]; - - int child = jcv_pq_maxchild(pq, pos); - while (child && jcv_halfedge_compare(node, items[child])) { - items[pos] = items[child]; - items[pos]->pqpos = pos; - pos = child; - child = jcv_pq_maxchild(pq, pos); - } - - items[pos] = node; - items[pos]->pqpos = pos; - return pos; -} - -static void jcv_pq_create(jcv_priorityqueue* pq, int capacity, void** buffer) { - pq->maxnumitems = capacity; - pq->numitems = 1; - pq->items = buffer; -} - -static int jcv_pq_empty(jcv_priorityqueue* pq) { return pq->numitems == 1 ? 1 : 0; } - -static int jcv_pq_push(jcv_priorityqueue* pq, void* node) { - assert(pq->numitems < pq->maxnumitems); - int n = pq->numitems++; - pq->items[n] = node; - return jcv_pq_moveup(pq, n); -} - -static void* jcv_pq_pop(jcv_priorityqueue* pq) { - void* node = pq->items[1]; - pq->items[1] = pq->items[--pq->numitems]; - jcv_pq_movedown(pq, 1); - return node; -} - -static void* jcv_pq_top(jcv_priorityqueue* pq) { return pq->items[1]; } - -static void jcv_pq_remove(jcv_priorityqueue* pq, jcv_halfedge* node) { - if (pq->numitems == 1) return; - int pos = node->pqpos; - if (pos == 0) return; - - jcv_halfedge** items = (jcv_halfedge**)pq->items; - - items[pos] = items[--pq->numitems]; - if (jcv_halfedge_compare(node, items[pos])) - jcv_pq_moveup(pq, pos); - else - jcv_pq_movedown(pq, pos); - node->pqpos = pos; -} - -// internal functions - -static inline jcv_site* jcv_nextsite(jcv_context_internal* internal) { return (internal->currentsite < internal->numsites) ? &internal->sites[internal->currentsite++] : 0; } - -static jcv_halfedge* jcv_get_edge_above_x(jcv_context_internal* internal, const jcv_point* p) { - // Gets the arc on the beach line at the x coordinate (i.e. right above the new site event) - - // A good guess it's close by (Can be optimized) - jcv_halfedge* he = internal->last_inserted; - if (!he) { - if (p->x < (internal->rect.max.x - internal->rect.min.x) / 2) - he = internal->beachline_start; - else - he = internal->beachline_end; - } - - // - if (he == internal->beachline_start || (he != internal->beachline_end && jcv_halfedge_rightof(he, p))) { - do { - he = he->right; - } while (he != internal->beachline_end && jcv_halfedge_rightof(he, p)); - - he = he->left; - } else { - do { - he = he->left; - } while (he != internal->beachline_start && !jcv_halfedge_rightof(he, p)); - } - - return he; -} - -static int jcv_check_circle_event(const jcv_halfedge* he1, const jcv_halfedge* he2, jcv_point* vertex) { - jcv_edge* e1 = he1->edge; - jcv_edge* e2 = he2->edge; - if (e1 == 0 || e2 == 0 || e1->sites[1] == e2->sites[1]) { - return 0; - } - - return jcv_halfedge_intersect(he1, he2, vertex); -} - -static void jcv_site_event(jcv_context_internal* internal, jcv_site* site) { - jcv_halfedge* left = jcv_get_edge_above_x(internal, &site->p); - jcv_halfedge* right = left->right; - jcv_site* bottom = jcv_halfedge_rightsite(left); - if (!bottom) bottom = internal->bottomsite; - - jcv_edge* edge = jcv_edge_new(internal, bottom, site); - edge->next = internal->edges; - internal->edges = edge; - - jcv_halfedge* edge1 = jcv_halfedge_new(internal, edge, JCV_DIRECTION_LEFT); - jcv_halfedge* edge2 = jcv_halfedge_new(internal, edge, JCV_DIRECTION_RIGHT); - - jcv_halfedge_link(left, edge1); - jcv_halfedge_link(edge1, edge2); - - internal->last_inserted = right; - - jcv_point p; - if (jcv_check_circle_event(left, edge1, &p)) { - jcv_pq_remove(internal->eventqueue, left); - left->vertex = p; - left->y = p.y + jcv_point_dist(&site->p, &p); - jcv_pq_push(internal->eventqueue, left); - } - if (jcv_check_circle_event(edge2, right, &p)) { - edge2->vertex = p; - edge2->y = p.y + jcv_point_dist(&site->p, &p); - jcv_pq_push(internal->eventqueue, edge2); - } -} - -// https://cp-algorithms.com/geometry/oriented-triangle-area.html -static inline jcv_real jcv_determinant(const jcv_point* a, const jcv_point* b, const jcv_point* c) { return (b->x - a->x) * (c->y - a->y) - (b->y - a->y) * (c->x - a->x); } - -static inline jcv_real jcv_calc_sort_metric(const jcv_site* site, const jcv_graphedge* edge) { - // We take the average of the two points, since we can better distinguish between very small edges - jcv_real half = 1 / (jcv_real)2; - jcv_real x = (edge->pos[0].x + edge->pos[1].x) * half; - jcv_real y = (edge->pos[0].y + edge->pos[1].y) * half; - jcv_real diffy = y - site->p.y; - jcv_real angle = JCV_ATAN2(diffy, x - site->p.x); - if (diffy < 0) angle = angle + 2 * JCV_PI; - return (jcv_real)angle; -} - -static void jcv_sortedges_insert(jcv_site* site, jcv_graphedge* edge) { - // Special case for the head end - if (site->edges == 0 || site->edges->angle >= edge->angle) { - edge->next = site->edges; - site->edges = edge; - } else { - // Locate the node before the point of insertion - jcv_graphedge* current = site->edges; - while (current->next != 0 && current->next->angle < edge->angle) { - current = current->next; - } - edge->next = current->next; - current->next = edge; - } -} - -static void jcv_finishline(jcv_context_internal* internal, jcv_edge* e) { - if (!jcv_edge_clipline(internal, e)) { - return; - } - - // Make sure the graph edges are CCW - int flip = jcv_determinant(&e->sites[0]->p, &e->pos[0], &e->pos[1]) > (jcv_real)0 ? 0 : 1; - - for (int i = 0; i < 2; ++i) { - jcv_graphedge* ge = jcv_alloc_graphedge(internal); - - ge->edge = e; - ge->next = 0; - ge->neighbor = e->sites[1 - i]; - ge->pos[flip] = e->pos[i]; - ge->pos[1 - flip] = e->pos[1 - i]; - ge->angle = jcv_calc_sort_metric(e->sites[i], ge); - - jcv_sortedges_insert(e->sites[i], ge); - - // check that we didn't accidentally add a duplicate (rare), then remove it - if (ge->next && ge->angle == ge->next->angle) { - if (jcv_point_eq(&ge->pos[0], &ge->next->pos[0]) && jcv_point_eq(&ge->pos[1], &ge->next->pos[1])) { - ge->next = ge->next->next; // Throw it away, they're so few anyways - } - } - } -} - -static void jcv_endpos(jcv_context_internal* internal, jcv_edge* e, const jcv_point* p, int direction) { - e->pos[direction] = *p; - - if (!jcv_is_valid(&e->pos[1 - direction])) return; - - jcv_finishline(internal, e); -} - -static inline void jcv_create_corner_edge(jcv_context_internal* internal, const jcv_site* site, jcv_graphedge* current, jcv_graphedge* gap) { - gap->neighbor = 0; - gap->pos[0] = current->pos[1]; - - if (current->pos[1].x < internal->rect.max.x && current->pos[1].y == internal->rect.min.y) { - gap->pos[1].x = internal->rect.max.x; - gap->pos[1].y = internal->rect.min.y; - } else if (current->pos[1].x > internal->rect.min.x && current->pos[1].y == internal->rect.max.y) { - gap->pos[1].x = internal->rect.min.x; - gap->pos[1].y = internal->rect.max.y; - } else if (current->pos[1].y > internal->rect.min.y && current->pos[1].x == internal->rect.min.x) { - gap->pos[1].x = internal->rect.min.x; - gap->pos[1].y = internal->rect.min.y; - } else if (current->pos[1].y < internal->rect.max.y && current->pos[1].x == internal->rect.max.x) { - gap->pos[1].x = internal->rect.max.x; - gap->pos[1].y = internal->rect.max.y; - } - - gap->angle = jcv_calc_sort_metric(site, gap); -} - -static jcv_edge* jcv_create_gap_edge(jcv_context_internal* internal, jcv_site* site, jcv_graphedge* ge) { - jcv_edge* edge = jcv_alloc_edge(internal); - edge->pos[0] = ge->pos[0]; - edge->pos[1] = ge->pos[1]; - edge->sites[0] = site; - edge->sites[1] = 0; - edge->a = edge->b = edge->c = 0; - edge->next = internal->edges; - internal->edges = edge; - return edge; -} - -void inline jcv_boxshape_fillgaps(const jcv_clipper* clipper, jcv_context_internal* allocator, jcv_site* site) { - // They're sorted CCW, so if the current->pos[1] != next->pos[0], then we have a gap - jcv_graphedge* current = site->edges; - if (!current) { - // No edges, then it should be a single cell - assert(allocator->numsites == 1); - - jcv_graphedge* gap = jcv_alloc_graphedge(allocator); - gap->neighbor = 0; - gap->pos[0] = clipper->min; - gap->pos[1].x = clipper->max.x; - gap->pos[1].y = clipper->min.y; - gap->angle = jcv_calc_sort_metric(site, gap); - gap->next = 0; - gap->edge = jcv_create_gap_edge(allocator, site, gap); - - current = gap; - site->edges = gap; - } - - jcv_graphedge* next = current->next; - if (!next) { - // Only one edge, then we assume it's a corner gap - jcv_graphedge* gap = jcv_alloc_graphedge(allocator); - jcv_create_corner_edge(allocator, site, current, gap); - gap->edge = jcv_create_gap_edge(allocator, site, gap); - - gap->next = current->next; - current->next = gap; - current = gap; - next = site->edges; - } - - while (current && next) { - if (jcv_point_on_box_edge(¤t->pos[1], &clipper->min, &clipper->max) && !jcv_point_eq(¤t->pos[1], &next->pos[0])) { - // Border gap - if (current->pos[1].x == next->pos[0].x || current->pos[1].y == next->pos[0].y) { - jcv_graphedge* gap = jcv_alloc_graphedge(allocator); - gap->neighbor = 0; - gap->pos[0] = current->pos[1]; - gap->pos[1] = next->pos[0]; - gap->angle = jcv_calc_sort_metric(site, gap); - gap->edge = jcv_create_gap_edge(allocator, site, gap); - - gap->next = current->next; - current->next = gap; - } else if (jcv_point_on_box_edge(¤t->pos[1], &clipper->min, &clipper->max) && jcv_point_on_box_edge(&next->pos[0], &clipper->min, &clipper->max)) { - jcv_graphedge* gap = jcv_alloc_graphedge(allocator); - jcv_create_corner_edge(allocator, site, current, gap); - gap->edge = jcv_create_gap_edge(allocator, site, gap); - gap->next = current->next; - current->next = gap; - } else { - // something went wrong, abort instead of looping indefinitely - break; - } - } - - current = current->next; - if (current) { - next = current->next; - if (!next) next = site->edges; - } - } -} - -// Since the algorithm leaves gaps at the borders/corner, we want to fill them -static void jcv_fillgaps(jcv_diagram* diagram) { - jcv_context_internal* internal = diagram->internal; - if (!internal->clipper.fill_fn) return; - - for (int i = 0; i < internal->numsites; ++i) { - jcv_site* site = &internal->sites[i]; - internal->clipper.fill_fn(&internal->clipper, internal, site); - } -} - -static void jcv_circle_event(jcv_context_internal* internal) { - jcv_halfedge* left = (jcv_halfedge*)jcv_pq_pop(internal->eventqueue); - - jcv_halfedge* leftleft = left->left; - jcv_halfedge* right = left->right; - jcv_halfedge* rightright = right->right; - jcv_site* bottom = jcv_halfedge_leftsite(left); - jcv_site* top = jcv_halfedge_rightsite(right); - - jcv_point vertex = left->vertex; - jcv_endpos(internal, left->edge, &vertex, left->direction); - jcv_endpos(internal, right->edge, &vertex, right->direction); - - internal->last_inserted = rightright; - - jcv_pq_remove(internal->eventqueue, right); - jcv_halfedge_unlink(left); - jcv_halfedge_unlink(right); - jcv_halfedge_delete(internal, left); - jcv_halfedge_delete(internal, right); - - int direction = JCV_DIRECTION_LEFT; - if (bottom->p.y > top->p.y) { - jcv_site* temp = bottom; - bottom = top; - top = temp; - direction = JCV_DIRECTION_RIGHT; - } - - jcv_edge* edge = jcv_edge_new(internal, bottom, top); - edge->next = internal->edges; - internal->edges = edge; - - jcv_halfedge* he = jcv_halfedge_new(internal, edge, direction); - jcv_halfedge_link(leftleft, he); - jcv_endpos(internal, edge, &vertex, JCV_DIRECTION_RIGHT - direction); - - jcv_point p; - if (jcv_check_circle_event(leftleft, he, &p)) { - jcv_pq_remove(internal->eventqueue, leftleft); - leftleft->vertex = p; - leftleft->y = p.y + jcv_point_dist(&bottom->p, &p); - jcv_pq_push(internal->eventqueue, leftleft); - } - if (jcv_check_circle_event(he, rightright, &p)) { - he->vertex = p; - he->y = p.y + jcv_point_dist(&bottom->p, &p); - jcv_pq_push(internal->eventqueue, he); - } -} - -static inline jcv_real jcv_floor(jcv_real v) { - jcv_real i = (jcv_real)(int)v; - return (v < i) ? i - 1 : i; -} - -static inline jcv_real jcv_ceil(jcv_real v) { - jcv_real i = (jcv_real)(int)v; - return (v > i) ? i + 1 : i; -} - -static inline jcv_real jcv_min(jcv_real a, jcv_real b) { return a < b ? a : b; } - -static inline jcv_real jcv_max(jcv_real a, jcv_real b) { return a > b ? a : b; } - -void inline jcv_diagram_generate(int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, jcv_diagram* d) { - jcv_diagram_generate_useralloc(num_points, points, rect, clipper, 0, jcv_alloc_fn, jcv_free_fn, d); -} - -typedef union _jcv_cast_align_struct { - char* charp; - void** voidpp; -} jcv_cast_align_struct; - -static inline void jcv_rect_union(jcv_rect* rect, const jcv_point* p) { - rect->min.x = jcv_min(rect->min.x, p->x); - rect->min.y = jcv_min(rect->min.y, p->y); - rect->max.x = jcv_max(rect->max.x, p->x); - rect->max.y = jcv_max(rect->max.y, p->y); -} - -static inline void jcv_rect_round(jcv_rect* rect) { - rect->min.x = jcv_floor(rect->min.x); - rect->min.y = jcv_floor(rect->min.y); - rect->max.x = jcv_ceil(rect->max.x); - rect->max.y = jcv_ceil(rect->max.y); -} - -static inline void jcv_rect_inflate(jcv_rect* rect, jcv_real amount) { - rect->min.x -= amount; - rect->min.y -= amount; - rect->max.x += amount; - rect->max.y += amount; -} - -static int jcv_prune_duplicates(jcv_context_internal* internal, const jcv_rect* rect) { - int num_sites = internal->numsites; - jcv_site* sites = internal->sites; - - jcv_rect r; - r.min.x = r.min.y = JCV_FLT_MAX; - r.max.x = r.max.y = -JCV_FLT_MAX; - - int offset = 0; - // Prune duplicates first - for (int i = 0; i < num_sites; i++) { - const jcv_site* s = &sites[i]; - // Remove duplicates, to avoid anomalies - if (i > 0 && jcv_point_eq(&s->p, &sites[i - 1].p)) { - offset++; - continue; - } - - sites[i - offset] = sites[i]; - - if (rect == 0) { - jcv_rect_union(&r, &s->p); - } - } - - if (rect == 0) { - jcv_rect_round(&r); - jcv_rect_inflate(&r, 10); - internal->rect = r; - } else { - internal->rect = *rect; - } - - return offset; -} - -static int jcv_prune_not_in_shape(jcv_context_internal* internal, const jcv_rect* rect) { - int num_sites = internal->numsites; - jcv_site* sites = internal->sites; - - jcv_rect r; - r.min.x = r.min.y = JCV_FLT_MAX; - r.max.x = r.max.y = -JCV_FLT_MAX; - - int offset = 0; - for (int i = 0; i < num_sites; i++) { - const jcv_site* s = &sites[i]; - - if (!internal->clipper.test_fn(&internal->clipper, s->p)) { - offset++; - continue; - } - - sites[i - offset] = sites[i]; - - if (rect == 0) { - jcv_rect_union(&r, &s->p); - } - } - - return offset; -} - -void inline jcv_diagram_generate_useralloc(int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, void* userallocctx, FJCVAllocFn allocfn, - FJCVFreeFn freefn, jcv_diagram* d) { - if (d->internal) jcv_diagram_free(d); - - // Interesting limits from Euler's equation - // Slide 81: https://courses.cs.washington.edu/courses/csep521/01au/lectures/lecture10slides.pdf - // Page 3: https://sites.cs.ucsb.edu/~suri/cs235/Voronoi.pdf - int max_num_events = num_points * 2; // beachline can have max 2*n-5 parabolas - size_t sitessize = (size_t)num_points * sizeof(jcv_site); - size_t memsize = 8u + (size_t)max_num_events * sizeof(void*) + sizeof(jcv_priorityqueue) + sitessize + sizeof(jcv_context_internal); - - char* originalmem = (char*)allocfn(userallocctx, memsize); - memset(originalmem, 0, memsize); - - // align memory - char* mem = originalmem + 8 - ((size_t)(originalmem)&0x7); - - jcv_context_internal* internal = (jcv_context_internal*)mem; - mem += sizeof(jcv_context_internal); - - internal->mem = originalmem; - internal->memctx = userallocctx; - internal->alloc = allocfn; - internal->free = freefn; - - internal->beachline_start = jcv_halfedge_new(internal, 0, 0); - internal->beachline_end = jcv_halfedge_new(internal, 0, 0); - - internal->beachline_start->left = 0; - internal->beachline_start->right = internal->beachline_end; - internal->beachline_end->left = internal->beachline_start; - internal->beachline_end->right = 0; - - internal->last_inserted = 0; - - internal->sites = (jcv_site*)mem; - mem += sitessize; - - internal->eventqueue = (jcv_priorityqueue*)mem; - mem += sizeof(jcv_priorityqueue); - - jcv_cast_align_struct tmp; - tmp.charp = mem; - internal->eventmem = tmp.voidpp; - - jcv_pq_create(internal->eventqueue, max_num_events, internal->eventmem); - - internal->numsites = num_points; - jcv_site* sites = internal->sites; - - for (int i = 0; i < num_points; ++i) { - sites[i].p = points[i]; - sites[i].edges = 0; - sites[i].index = i; - } - - qsort(sites, (size_t)num_points, sizeof(jcv_site), jcv_point_cmp); - - jcv_clipper box_clipper; - if (clipper == 0) { - box_clipper.test_fn = jcv_boxshape_test; - box_clipper.clip_fn = jcv_boxshape_clip; - box_clipper.fill_fn = jcv_boxshape_fillgaps; - clipper = &box_clipper; - } - internal->clipper = *clipper; - - int offset = jcv_prune_duplicates(internal, rect); - num_points -= offset; - - // Prune using the test second - if (internal->clipper.test_fn) { - internal->clipper.min = internal->rect.min; - internal->clipper.max = internal->rect.max; - - offset = jcv_prune_not_in_shape(internal, rect); - num_points -= offset; - - internal->clipper.min = internal->rect.min; - internal->clipper.max = internal->rect.max; - } - - d->min = internal->rect.min; - d->max = internal->rect.max; - d->internal = internal; - d->numsites = num_points; - - internal->numsites = num_points; - internal->currentsite = 0; - - internal->bottomsite = jcv_nextsite(internal); - - jcv_priorityqueue* pq = internal->eventqueue; - jcv_site* site = jcv_nextsite(internal); - - int finished = 0; - while (!finished) { - jcv_point lowest_pq_point; - if (!jcv_pq_empty(pq)) { - jcv_halfedge* he = (jcv_halfedge*)jcv_pq_top(pq); - lowest_pq_point.x = he->vertex.x; - lowest_pq_point.y = he->y; - } - - if (site != 0 && (jcv_pq_empty(pq) || jcv_point_less(&site->p, &lowest_pq_point))) { - jcv_site_event(internal, site); - site = jcv_nextsite(internal); - } else if (!jcv_pq_empty(pq)) { - jcv_circle_event(internal); - } else { - finished = 1; - } - } - - for (jcv_halfedge* he = internal->beachline_start->right; he != internal->beachline_end; he = he->right) { - jcv_finishline(internal, he->edge); - } - - jcv_fillgaps(d); -} - -#endif // JC_VORONOI_IMPLEMENTATION - -/* - -ABOUT: - - A fast single file 2D voronoi diagram generator - -HISTORY: - - 0.7 2019-10-25 - Added support for clipping against convex polygons - - Added JCV_EDGE_INTERSECT_THRESHOLD for edge intersections - - Fixed issue where the bounds calculation wasn’t considering all points - 0.6 2018-10-21 - Removed JCV_CEIL/JCV_FLOOR/JCV_FABS - - Optimizations: Fewer indirections, better beach head approximation - 0.5 2018-10-14 - Fixed issue where the graph edge had the wrong edge assigned (issue #28) - - Fixed issue where a point was falsely passing the jcv_is_valid() test (issue #22) - - Fixed jcv_diagram_get_edges() so it now returns _all_ edges (issue #28) - - Added jcv_diagram_get_next_edge() to skip zero length edges (issue #10) - - Added defines JCV_CEIL/JCV_FLOOR/JCV_FLT_MAX for easier configuration - 0.4 2017-06-03 - Increased the max number of events that are preallocated - 0.3 2017-04-16 - Added clipping box as input argument (Automatically calculated if needed) - - Input points are pruned based on bounding box - 0.2 2016-12-30 - Fixed issue of edges not being closed properly - - Fixed issue when having many events - - Fixed edge sorting - - Code cleanup - 0.1 Initial version - -LICENSE: - - The MIT License (MIT) - - Copyright (c) 2015-2019 Mathias Westerdahl - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. - - -DISCLAIMER: - - This software is supplied "AS IS" without any warranties and support - -USAGE: - - The input points are pruned if - - * There are duplicates points - * The input points are outside of the bounding box (i.e. fail the clipping test function) - * The input points are rejected by the clipper's test function - - The input bounding box is optional (calculated automatically) - - The input domain is (-FLT_MAX, FLT_MAX] (for floats) - - The api consists of these functions: - - void jcv_diagram_generate( int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, jcv_diagram* diagram ); - void jcv_diagram_generate_useralloc( int num_points, const jcv_point* points, const jcv_rect* rect, const jcv_clipper* clipper, const jcv_clipper* clipper, void* userallocctx, -FJCVAllocFn allocfn, FJCVFreeFn freefn, jcv_diagram* diagram ); void jcv_diagram_free( jcv_diagram* diagram ); - - const jcv_site* jcv_diagram_get_sites( const jcv_diagram* diagram ); - const jcv_edge* jcv_diagram_get_edges( const jcv_diagram* diagram ); - const jcv_edge* jcv_diagram_get_next_edge( const jcv_edge* edge ); - - An example usage: - - #define JC_VORONOI_IMPLEMENTATION - // If you wish to use doubles - //#define JCV_REAL_TYPE double - //#define JCV_ATAN2 atan2 - //#define JCV_FLT_MAX 1.7976931348623157E+308 - #include "jc_voronoi.h" - - void draw_edges(const jcv_diagram* diagram); - void draw_cells(const jcv_diagram* diagram); - - void generate_and_draw(int numpoints, const jcv_point* points) - { - jcv_diagram diagram; - memset(&diagram, 0, sizeof(jcv_diagram)); - jcv_diagram_generate(count, points, 0, 0, &diagram); - - draw_edges(diagram); - draw_cells(diagram); - - jcv_diagram_free( &diagram ); - } - - void draw_edges(const jcv_diagram* diagram) - { - // If all you need are the edges - const jcv_edge* edge = jcv_diagram_get_edges( diagram ); - while( edge ) - { - draw_line(edge->pos[0], edge->pos[1]); - edge = jcv_diagram_get_next_edge(edge); - } - } - - void draw_cells(const jcv_diagram* diagram) - { - // If you want to draw triangles, or relax the diagram, - // you can iterate over the sites and get all edges easily - const jcv_site* sites = jcv_diagram_get_sites( diagram ); - for( int i = 0; i < diagram->numsites; ++i ) - { - const jcv_site* site = &sites[i]; - - const jcv_graphedge* e = site->edges; - while( e ) - { - draw_triangle( site->p, e->pos[0], e->pos[1]); - e = e->next; - } - } - } - - // Here is a simple example of how to do the relaxations of the cells - void relax_points(const jcv_diagram* diagram, jcv_point* points) - { - const jcv_site* sites = jcv_diagram_get_sites(diagram); - for( int i = 0; i < diagram->numsites; ++i ) - { - const jcv_site* site = &sites[i]; - jcv_point sum = site->p; - int count = 1; - - const jcv_graphedge* edge = site->edges; - - while( edge ) - { - sum.x += edge->pos[0].x; - sum.y += edge->pos[0].y; - ++count; - edge = edge->next; - } - - points[site->index].x = sum.x / count; - points[site->index].y = sum.y / count; - } - } - - */ diff --git a/include/roboteam_ai/control/positionControl/pathPlanning/VoronoiPathPlanning.h b/include/roboteam_ai/control/positionControl/pathPlanning/VoronoiPathPlanning.h deleted file mode 100644 index 3a0ea2420c..0000000000 --- a/include/roboteam_ai/control/positionControl/pathPlanning/VoronoiPathPlanning.h +++ /dev/null @@ -1,72 +0,0 @@ -//; -// Created by ratoone on 05-11-19. -// - -#ifndef RTT_VORONOIPATHPLANNING_H -#define RTT_VORONOIPATHPLANNING_H - -#include -#include - -#include "Voronoi.h" -#include "control/ControlUtils.h" -#include "control/positionControl/CollisionDetector.h" -#include "roboteam_utils/Vector2.h" -#include "utilities/Constants.h" - -namespace rtt::ai::control { -struct GraphNode { - Vector2 nextNodePosition; - double distance; -}; - -/** - * Path planning algorithm. See method computePath for details. - */ -class VoronoiPathPlanning { - private: - jcv_diagram voronoiDiagram{}; - - CollisionDetector &collisionDetector; - - /// the adjacency list is a map from the position of the node to the adjacent nodes - std::unordered_map> graphAdjacencyList; - - /// helper function to transport a point structure to a Vector2 object - Vector2 convertFromJcvPoint(jcv_point point); - - /// generate an unoriented graph from the Voronoi diagram - void generateGraphFromDiagram(); - - /// using Dijkstra's algorithm, find the path between the initial and target points in the graph - std::vector generatePathDijkstra(const Vector2 &initialPosition, const Vector2 &targetPosition); - - /// computes the Voronoi diagram, taking into account the start and end positions for clipping - void computeDiagram(const Vector2 &robotPosition, const Vector2 &targetPosition); - - /// add the specified point to the graph - void addPointToGraph(const Vector2 &pointToAdd); - - public: - /** - * The collision detector is provided by the position control. This class was intended - * to be used only with the PositionControl - * @param collisionDetector - */ - explicit VoronoiPathPlanning(CollisionDetector &collisionDetector); - - /** - * Computes a path using the implemented algorithm. It takes into account the - * obstacles present in the field.

- * VoronoiPathPlanning generates the Voronoi diagram using the robots in the rectangle - * between the initial and final position. Then it creates a graph with the resulting lines - * and using Dijkstra, it computes the shortest graph path. - * @param robotPosition the current robot position - * @param targetPosition the goal position - * @return a list of points representing the path - */ - std::vector computePath(const Vector2 &robotPosition, const Vector2 &targetPosition); -}; -} // namespace rtt::ai::control - -#endif // RTT_VORONOIPATHPLANNING_H diff --git a/include/roboteam_ai/control/positionControl/pathTracking/BBTPathTracking.h b/include/roboteam_ai/control/positionControl/pathTracking/BBTPathTracking.h index 56ad5b9cea..ae1d709409 100644 --- a/include/roboteam_ai/control/positionControl/pathTracking/BBTPathTracking.h +++ b/include/roboteam_ai/control/positionControl/pathTracking/BBTPathTracking.h @@ -5,38 +5,91 @@ #ifndef RTT_BBTPATHTRACKING_H #define RTT_BBTPATHTRACKING_H -#include "PidTracking.h" +#include + +#include "control/positionControl/BBTrajectories/BBTrajectory2D.h" +#include "control/positionControl/CollisionDetector.h" #include "control/positionControl/PositionControlUtils.h" #include "roboteam_utils/Position.h" #include "roboteam_utils/Vector2.h" +#include "roboteam_utils/pid.h" #include "utilities/Constants.h" namespace rtt::ai::control { +enum UpdatePath { + DONT_UPDATE, + UPDATE_TARGET_CHANGED, + UPDATE_TARGET_REACHED, + + UPDATE_COLLISION_DETECTED, +}; + +inline std::ostream& operator<<(std::ostream& os, const UpdatePath updatePath) { + switch (updatePath) { + case UpdatePath::DONT_UPDATE: + os << "DONT_UPDATE"; + break; + case UpdatePath::UPDATE_TARGET_CHANGED: + os << "UPDATE_TARGET_CHANGED"; + break; + case UpdatePath::UPDATE_TARGET_REACHED: + os << "UPDATE_TARGET_REACHED"; + break; + case UpdatePath::UPDATE_COLLISION_DETECTED: + os << "UPDATE_COLLISION_DETECTED"; + break; + } + return os; +} + /** - * Path tracking algorithm for BBT + * Path tracker for single robot. */ class BBTPathTracking { private: - static constexpr unsigned long STEPS_AHEAD = 1; - PidTracking pidTracking; + static constexpr size_t STEPS_AHEAD = 1; + const int robotId; + const CollisionDetector& collisionDetector; + + PID xPID; + PID yPID; + + std::vector path; + std::span remainingPath; public: + BBTPathTracking(int robotId, const CollisionDetector& collisionDetector); + + /** + * \brief Determine if the path needs to be recomputed (e.g. when target has changed) + * @param currentPos Current position of the robot + * @param targetPos Target position of the robot from STP + * @param avoidObjects Objects deemed to be obstacles for collision detection + * + * @returns UpdatePath::DONT_UPDATE (i.e. 0) if the path is up to date else ret_val > 0 + */ + [[nodiscard]] UpdatePath shouldUpdatePath(const Vector2& currentPos, const Vector2& targetPos, const stp::AvoidObjects& avoidObjects); + + /** + * \brief Update the path + * @param newPath New path to be used + */ + void updatePath(std::vector&& newPath); + + /** + * \brief Computes and returns next position and angle to be send to the robot + * @param currentPos Current position of the robot + * @param currentVel Current velocity of the robot + * @param pidType PID type to be used (TODO: What is pid type?) + * @returns Next position and velocity to be send to the robot + */ + [[nodiscard]] Position trackPathForwardAngle(const Vector2& currentPosition, const Vector2& currentVelocity, stp::PIDType pidType); + /** - * Generates an output velocity and angle according to the implemented algorithm. - * After reaching a certain distance to the closest path point, it will go to the next one.

- * DensePathTracking looks at the target position after N steps and follows that - * using a PID controller (the PidTracking). It is assumed that the path is dense. - * @param currentPosition - * @param currentVelocity - * @param pathPoints the path as a list of points - * @param velocityPoints the path as a list of velocities - * @param robotId the ID of the current robot - * @param angle the desired orientation angle of the robot - if omitted, the robot will face its velocity - * @return a structure containing the tracking velocity and the orientation angle + * \brief Returns readonly view of the remaining path */ - Position trackPathForwardAngle(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector> &pathVelocityPoints, int robotId, - stp::PIDType pidType); + [[nodiscard]] std::span getRemainingPath(); }; } // namespace rtt::ai::control diff --git a/include/roboteam_ai/control/positionControl/pathTracking/DensePathTracking.h b/include/roboteam_ai/control/positionControl/pathTracking/DensePathTracking.h deleted file mode 100644 index b0cd0a1684..0000000000 --- a/include/roboteam_ai/control/positionControl/pathTracking/DensePathTracking.h +++ /dev/null @@ -1,45 +0,0 @@ -// -// Created by ratoone on 24-01-20. -// - -#ifndef RTT_DENSEPATHTRACKING_H -#define RTT_DENSEPATHTRACKING_H - -#include - -#include "PathTrackingAlgorithm.h" -#include "PidTracking.h" -#include "roboteam_utils/Position.h" -#include "roboteam_utils/Vector2.h" -#include "utilities/Constants.h" - -namespace rtt::ai::control { - -/** - * Path tracking algorithm. See method computePath for details. - */ -class DensePathTracking : public PathTrackingAlgorithm { - private: - static constexpr unsigned long STEPS_AHEAD = 1; - - PidTracking pidTracking; - - public: - /** - * Generates an output velocity and angle according to the implemented algorithm. - * After reaching a certain distance to the closest path point, it will go to the next one.

- * DensePathTracking looks at the target position after N steps and follows that - * using a PID controller (the PidTracking). It is assumed that the path is dense. - * @param currentPosition - * @param currentVelocity - * @param pathPoints the path as a list of points - * @param robotId the ID of the current robot - * @param angle the desired orientation angle of the robot - if omitted, the robot will face its velocity - * @return a structure containing the tracking velocity and the orientation angle - */ - Position trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) override; -}; - -} // namespace rtt::ai::control - -#endif // RTT_DENSEPATHTRACKING_H diff --git a/include/roboteam_ai/control/positionControl/pathTracking/PathTrackingAlgorithm.h b/include/roboteam_ai/control/positionControl/pathTracking/PathTrackingAlgorithm.h deleted file mode 100644 index 167d7471cf..0000000000 --- a/include/roboteam_ai/control/positionControl/pathTracking/PathTrackingAlgorithm.h +++ /dev/null @@ -1,43 +0,0 @@ -// -// Created by ratoone on 09-03-20. -// - -#ifndef RTT_PATHTRACKINGALGORITHM_H -#define RTT_PATHTRACKINGALGORITHM_H - -#include -#include - -#include - -namespace rtt::ai::control { -class PathTrackingAlgorithm { - public: - /** - * Purely virtual function that will handle tracking of a path - * @param currentPosition - * @param currentVelocity - * @param pathPoints - * @param robotId - * @param angle - * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @return a structure containing the tracking velocity and the orientation angle - */ - virtual Position trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, double angle, - stp::PIDType pidType) = 0; - - /** - * Uses the implementation of trackPath, but replaces the angle with the orientation of the - * direction of the current position and the next point in the path - * @param currentPosition - * @param currentVelocity - * @param pathPoints - * @param robotId - * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @return a structure containing the tracking velocity and the orientation angle - */ - Position trackPathDefaultAngle(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, stp::PIDType pidType); -}; -} // namespace rtt::ai::control - -#endif // RTT_PATHTRACKINGALGORITHM_H diff --git a/include/roboteam_ai/control/positionControl/pathTracking/PidTracking.h b/include/roboteam_ai/control/positionControl/pathTracking/PidTracking.h deleted file mode 100644 index cd3d4b95e2..0000000000 --- a/include/roboteam_ai/control/positionControl/pathTracking/PidTracking.h +++ /dev/null @@ -1,55 +0,0 @@ -// -// Created by ratoone on 24-01-20. -// - -#ifndef RTT_PIDTRACKING_H -#define RTT_PIDTRACKING_H - -#include "PathTrackingAlgorithm.h" -#include "control/positionControl/PositionControlUtils.h" -#include "interface/api/Output.h" -#include "roboteam_utils/Position.h" -#include "roboteam_utils/Vector2.h" -#include "roboteam_utils/pid.h" -#include "utilities/Constants.h" - -namespace rtt::ai::control { - -/** - * Path tracking algorithm. See method computePath for details. - */ -class PidTracking : public PathTrackingAlgorithm { - private: - static constexpr double MAX_VELOCITY = Constants::MAX_VEL(); - - // PID controllers for each robot - std::unordered_map> pidMapping = {}; - - // updates the PID parameters from the UI - void updatePidValuesFromInterface(bool isKeeper); - - public: - /** - * Generates an output velocity and angle according to the implemented algorithm. - * After reaching a certain distance to the closest path point, it will go to the next one.

- * PidTracking applied a PID to the difference of positions, to obtain a velocity. The values are - * taken from the interface. - * @param currentPosition - * @param currentVelocity - * @param pathPoints the path as a list of points - * @param robotId the robotId of the controlled robot - used only for state based tracking (like PID) - * @param angle the desired orientation angle of the robot - if omitted, the robot will face its velocity - * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @return a structure containing the tracking velocity and the orientation angle - */ - Position trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, double angle, stp::PIDType pidType) override; - /** - * Update the PID values for a specific robot - * @param pidType The desired PID type (intercept, regular, keeper etc.) - * @param robotID ID of the robot that we want to change the PID for - */ - void updatePIDValues(stp::PIDType pidType, int robotID); -}; -} // namespace rtt::ai::control - -#endif // RTT_PIDTRACKING_H diff --git a/include/roboteam_ai/interface/api/Input.h b/include/roboteam_ai/interface/api/Input.h index d45407222e..dd1971425d 100644 --- a/include/roboteam_ai/interface/api/Input.h +++ b/include/roboteam_ai/interface/api/Input.h @@ -10,9 +10,11 @@ #include #include #include +#include #include #include "Toggles.h" +#include "control/positionControl/BBTrajectories/BBTrajectory2D.h" namespace rtt::ai::interface { @@ -49,6 +51,8 @@ class Input { static const std::vector getDrawings(); static void drawData(Visual visual, std::vector points, QColor color, int robotId = -1, Drawing::DrawingMethod method = Drawing::DOTS, double width = 4.0, double height = 4.0, double strokeWidth = 2.0); + static void drawData(Visual visual, std::span points, QColor color, int robotId = -1, Drawing::DrawingMethod method = Drawing::DOTS, double width = 4.0, + double height = 4.0, double strokeWidth = 2.0); static void drawDebugData(std::vector points, QColor color = Qt::yellow, int robotId = -1, Drawing::DrawingMethod method = Drawing::DOTS, double width = 4.0, double height = 4.0, double strokeWidth = 4.0); static int getFps(); diff --git a/include/roboteam_ai/utilities/Constants.h b/include/roboteam_ai/utilities/Constants.h index dd83f8825b..c0f83694ee 100644 --- a/include/roboteam_ai/utilities/Constants.h +++ b/include/roboteam_ai/utilities/Constants.h @@ -68,6 +68,14 @@ class Constants { static double CENTRE_TO_FRONT(); static double CLOSE_TO_BORDER_DISTANCE(); + // POSITION CONTROL // + static double POSITION_CONTROL_MAX_TARGET_DEVIATION(); + static double POSITION_CONTROL_MAX_STILL_VELOCITY(); + static double POSITION_CONTROL_MIN_DISTANCE_REACHED(); + static constexpr int POSITION_CONTROL_TIME_STEP() { return 100; }; + static constexpr int POSITION_CONTROL_SCAN_RANGE() { return 2500; }; + static constexpr int POSITION_CONTROL_STEP_COUNT() { return Constants::POSITION_CONTROL_SCAN_RANGE() / Constants::POSITION_CONTROL_TIME_STEP(); }; + static double HAS_BALL_DISTANCE(); static double HAS_BALL_ANGLE(); diff --git a/src/STPManager.cpp b/src/STPManager.cpp index 5176f6bdcf..6ad27a6971 100644 --- a/src/STPManager.cpp +++ b/src/STPManager.cpp @@ -4,6 +4,8 @@ #include #include +#include + #include #include "control/ControlModule.h" @@ -99,20 +101,14 @@ void STPManager::start() { roboteam_utils::Timer stpTimer; stpTimer.loop( [&]() { - // uncomment the 4 lines of code below to time and display the duration of each loop of the AI - // std::chrono::steady_clock::time_point tStart = std::chrono::steady_clock::now(); + ZoneScopedN("STP Loop"); runOneLoopCycle(); - // std::chrono::steady_clock::time_point tStop = std::chrono::steady_clock::now(); - - // auto loopcycleDuration = std::chrono::duration_cast((tStop - tStart)).count(); - // RTT_DEBUG("Loop cycle duration = ", loopcycleDuration); amountOfCycles++; - - // update the measured FPS, but limit this function call to only run 5 times/s at most int fpsUpdateRate = 5; stpTimer.limit( [&]() { ai::interface::Input::setFps(amountOfCycles * fpsUpdateRate); + ai::interface::Input::clearDrawings(); amountOfCycles = 0; }, fpsUpdateRate); diff --git a/src/control/ControlModule.cpp b/src/control/ControlModule.cpp index 0cbc992a33..101e5d2c33 100644 --- a/src/control/ControlModule.cpp +++ b/src/control/ControlModule.cpp @@ -74,8 +74,8 @@ void ControlModule::addRobotCommand(std::optional<::rtt::world::view::RobotView> if (robot && robot->get()) { Angle target = command.targetAngle; - interface::Input::drawData(interface::Visual::PATHFINDING, {robot->get()->getPos(), robot->get()->getPos() + Vector2(target)}, Qt::red, robot->get()->getId(), - interface::Drawing::LINES_CONNECTED); + // interface::Input::drawData(interface::Visual::PATHFINDING, {robot->get()->getPos(), robot->get()->getPos() + Vector2(target)}, Qt::red, robot->get()->getId(), + // interface::Drawing::LINES_CONNECTED); } // If we are not left, commands should be rotated (because we play as right) if (!SETTINGS.isLeft()) { diff --git a/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp b/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp index 8ab48de0aa..b0d42a0376 100644 --- a/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp +++ b/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp @@ -9,13 +9,10 @@ #include "utilities/Constants.h" namespace rtt::BB { -BBTrajectory2D::BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double alpha) { - generateTrajectory(initialPos, initialVel, finalPos, maxVel, maxAcc, alpha); -} - -BBTrajectory2D::BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc) { +BBTrajectory2D::BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double timeStep) : timeStep(timeStep) { generateSyncedTrajectory(initialPos, initialVel, finalPos, maxVel, maxAcc); } + void BBTrajectory2D::generateTrajectory(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc, double alpha) { x = BBTrajectory1D(initialPos.x, initialVel.x, finalPos.x, maxVel * cos(alpha), maxAcc * cos(alpha)); y = BBTrajectory1D(initialPos.y, initialVel.y, finalPos.y, maxVel * sin(alpha), maxAcc * sin(alpha)); @@ -50,32 +47,40 @@ Vector2 BBTrajectory2D::getVelocity(double t) const { return Vector2(x.getVeloci Vector2 BBTrajectory2D::getAcceleration(double t) const { return Vector2(x.getAcceleration(t), y.getAcceleration(t)); } -std::vector BBTrajectory2D::getStraightLines(unsigned int N) const { - std::vector points; - double timeStep = fmax(x.getTotalTime(), y.getTotalTime()) / N; - for (unsigned int i = 0; i <= N; ++i) { - points.push_back(getPosition(timeStep * i)); +// std::vector BBTrajectory2D::getStraightLines(unsigned int N) const { +// std::vector points; +// double timeStep = fmax(x.getTotalTime(), y.getTotalTime()) / N; +// for (unsigned int i = 0; i <= N; ++i) { +// points.push_back(getPosition(timeStep * i)); +// } +// return points; +// } + +std::vector &BBTrajectory2D::getPathApproach() { + if (!positions.empty()) { + return positions; } - return points; -} -std::vector BBTrajectory2D::getPathApproach(double timeStep) const { - std::vector points; auto totalTime = getTotalTime(); + positions.reserve(totalTime / timeStep + 1); double time = 0; while (time < totalTime) { time += timeStep; - points.push_back(getPosition(time)); + positions.push_back(getPosition(time)); } - return points; + return positions; } double BBTrajectory2D::getTotalTime() const { return std::max(x.getTotalTime(), y.getTotalTime()); } -std::vector BBTrajectory2D::getVelocityVector(double timeStep) const { - std::vector velocities; +std::vector &BBTrajectory2D::getVelocityVector() { + if (!velocities.empty()) { + return velocities; + } + auto totalTime = getTotalTime(); + velocities.reserve(totalTime / timeStep + 1); double time = 0; while (time < totalTime) { @@ -85,15 +90,14 @@ std::vector BBTrajectory2D::getVelocityVector(double timeStep) const { return velocities; } -std::vector> BBTrajectory2D::getPosVelVector(double timeStep) { - auto posVector = getPathApproach(timeStep); - auto velVector = getVelocityVector(timeStep); - - std::vector> posVelVector; +std::vector BBTrajectory2D::getPosVelVector() { + const auto &posVector = getPathApproach(); + const auto &velVector = getVelocityVector(); + auto posVelVector = std::vector{}; posVelVector.reserve(posVector.size()); for (size_t i = 0; i < posVector.size(); i++) { - posVelVector.emplace_back(std::make_pair(posVector[i], velVector[i])); + posVelVector.emplace_back(PosVelVector{posVector[i], velVector[i]}); } return posVelVector; diff --git a/src/control/positionControl/BBTrajectories/Trajectory1D.cpp b/src/control/positionControl/BBTrajectories/Trajectory1D.cpp deleted file mode 100644 index 72324159e0..0000000000 --- a/src/control/positionControl/BBTrajectories/Trajectory1D.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// -// Created by tijmen on 16-12-21. -// - -#include "control/positionControl/BBTrajectories/Trajectory1D.h" - -#include - -namespace rtt { - -void Trajectory1D::addTrajectory(const std::vector& newParts, double addFromTime) { - for (size_t i = 0; i < parts.size(); i++) { - if (addFromTime <= parts[i].tEnd) { - parts[i].tEnd = addFromTime; - parts.erase(parts.begin() + 1 + i, parts.end()); - break; - } - } - - for (BB::BBTrajectoryPart newPart : newParts) { - newPart.tEnd += addFromTime; - parts.push_back(newPart); - } -} - -double Trajectory1D::getAcceleration(double t) const { - double trajTime = fmax(0, t); - if (trajTime >= getTotalTime()) { - return 0; - } - BB::BBTrajectoryPart piece = parts[0]; - // we step through the parts and try to find the relevant part on which the time is. - for (const auto& part : parts) { - piece = part; - if (trajTime <= part.tEnd) { - break; - } - } - return piece.acc; -} - -double Trajectory1D::getVelocity(double t) const { - double trajTime = fmax(0, t); - BB::BBTrajectoryPart piece = parts[0]; - if (trajTime >= getTotalTime()) { - // The time is not on the trajectory so we just return the last known element - return 0; - } - // we step through the parts and try to find the relevant part on which the time is. - double tPieceStart = 0; - for (const auto& part : parts) { - piece = part; - if (trajTime <= part.tEnd) { - break; - } - tPieceStart = part.tEnd; - } - double tPiece = trajTime - tPieceStart; - // extrapolate the state given the information we have. - return piece.startVel + piece.acc * tPiece; -} - -double Trajectory1D::getPosition(double t) const { - double trajTime = fmax(0, t); - BB::BBTrajectoryPart piece = parts[0]; - if (trajTime >= getTotalTime()) { - return finalPos; - } - // we step through the parts and try to find the relevant part on which the time is. - double tPieceStart = 0; - for (const auto& part : parts) { - piece = part; - if (trajTime <= part.tEnd) { - break; - } - tPieceStart = part.tEnd; - } - double tPiece = trajTime - tPieceStart; - // extrapolate the state given the information we have. - return piece.startPos + piece.startVel * tPiece + 0.5 * piece.acc * tPiece * tPiece; -} - -double Trajectory1D::getTotalTime() const { return parts[parts.size() - 1].tEnd; } - -} // namespace rtt \ No newline at end of file diff --git a/src/control/positionControl/BBTrajectories/Trajectory2D.cpp b/src/control/positionControl/BBTrajectories/Trajectory2D.cpp deleted file mode 100644 index e033739d3f..0000000000 --- a/src/control/positionControl/BBTrajectories/Trajectory2D.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// -// Created by tijmen on 16-12-21. -// - -#include "control/positionControl/BBTrajectories/Trajectory2D.h" - -#include - -#include "control/positionControl/BBTrajectories/BBTrajectory2D.h" -#include "roboteam_utils/Print.h" - -namespace rtt { - -Trajectory2D::Trajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc) { - BB::BBTrajectory2D BBTNoCollision = BB::BBTrajectory2D(initialPos, initialVel, finalPos, maxVel, maxAcc); - std::pair, std::vector> parts = BBTNoCollision.getParts(); - x.parts = parts.first; - x.initialPos = initialPos.x; - x.initialVel = initialVel.x; - x.finalPos = finalPos.x; - y.parts = parts.second; - y.initialPos = initialPos.y; - y.initialVel = initialVel.y; - y.finalPos = finalPos.y; -} - -void Trajectory2D::addTrajectory(const Trajectory2D &extraTrajectory, double addFromTime) { - x.addTrajectory(extraTrajectory.x.parts, addFromTime); - x.finalPos = extraTrajectory.x.finalPos; - y.addTrajectory(extraTrajectory.y.parts, addFromTime); - y.finalPos = extraTrajectory.y.finalPos; -} - -std::vector Trajectory2D::getPathApproach(double timeStep) const { - std::vector points; - auto totalTime = getTotalTime(); - double time = 0; - - if (totalTime == std::numeric_limits::infinity()) { - // TODO: Prevent this from happening! - RTT_ERROR("Infinite while loop") - // throw std::runtime_error("Total time of infinity!"); - return {getPosition(0)}; - } - - while (time <= totalTime) { - time += timeStep; - points.push_back(getPosition(time)); - } - return points; -} - -std::vector Trajectory2D::getVelocityVector(double timeStep) const { - std::vector velocities; - auto totalTime = getTotalTime(); - double time = 0; - - if (totalTime == std::numeric_limits::infinity()) { - // TODO: Prevent this from happening! - RTT_ERROR("Infinite while loop") - // throw std::runtime_error("Total time of infinity!"); - return {getPosition(0)}; - } - - while (time <= totalTime) { - time += timeStep; - velocities.push_back(getVelocity(time)); - } - return velocities; -} - -Vector2 Trajectory2D::getPosition(double t) const { return Vector2(x.getPosition(t), y.getPosition(t)); } - -Vector2 Trajectory2D::getVelocity(double t) const { return Vector2(x.getVelocity(t), y.getVelocity(t)); } - -Vector2 Trajectory2D::getAcceleration(double t) const { return Vector2(x.getAcceleration(t), y.getAcceleration(t)); } - -double Trajectory2D::getTotalTime() const { return std::max(x.getTotalTime(), y.getTotalTime()); } - -} // namespace rtt \ No newline at end of file diff --git a/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/src/control/positionControl/BBTrajectories/WorldObjects.cpp deleted file mode 100644 index 6844ba00bb..0000000000 --- a/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// -// Created by floris on 15-11-20. -// -#include "control/positionControl/BBTrajectories/WorldObjects.h" - -#include -#include - -#include "world/World.hpp" -#include "world/WorldData.hpp" - -namespace rtt::BB { - -WorldObjects::WorldObjects() = default; - -std::optional WorldObjects::getFirstCollision(const rtt::world::World *world, const rtt::world::Field &field, const Trajectory2D &Trajectory, - const std::unordered_map> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects) { - gameState = rtt::ai::GameStateManager::getCurrentGameState(); - ruleset = gameState.getRuleSet(); - // TODO: return the kind of collision - //^-Question from Max not high priority - // TODO: find a good value for the timeStep - double timeStep = 0.1; - auto pathPoints = Trajectory.getPathApproach(timeStep); - - size_t timeStepsDone; - computedPaths.contains(robotId) && pathPoints.size() > computedPaths.at(robotId).size() ? timeStepsDone = pathPoints.size() - computedPaths.at(robotId).size() - : timeStepsDone = 0; - - std::vector collisionDatas; - - // If the robot can not move outside the field, check if its path goes outside the field - if (avoidObjects.shouldAvoidOutOfField) { - calculateFieldCollisions(field, collisionDatas, pathPoints, robotId, timeStep); - } - - // If the robot can not move into defense area, check if its path goes into either defense area. Don't check if the robot is in the defense are. - if (avoidObjects.shouldAvoidDefenseArea) { - calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, robotId, timeStep); - } - - // Check if robot is closer to the ball than it is allowed to be - if (avoidObjects.shouldAvoidBall) { - calculateBallCollisions(world, collisionDatas, pathPoints, timeStep); - } - - // Loop through all pathPoints for each enemy robot, and check if a point in the path will collide with an enemy robot - calculateEnemyRobotCollisions(world, Trajectory, collisionDatas, pathPoints, timeStep, timeStepsDone); - - // For each path already calculated, check if this path collides with those paths - calculateOurRobotCollisions(world, collisionDatas, pathPoints, computedPaths, robotId, timeStep, timeStepsDone); - - if (!collisionDatas.empty()) { - return collisionDatas[0]; - } else { - return std::nullopt; - } -} - -void WorldObjects::calculateFieldCollisions(const rtt::world::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, - double timeStep) { - for (size_t i = 0; i < pathPoints.size(); i++) { - if (!rtt::ai::FieldComputations::pointIsInField(field, pathPoints[i], rtt::ai::Constants::ROBOT_RADIUS())) { - // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). - if (i == 0) return; - - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], pathPoints[i], i * timeStep, "FieldCollision"}); - return; - } - } -} - -void WorldObjects::calculateDefenseAreaCollisions(const rtt::world::Field &field, std::vector &collisionDatas, const std::vector &pathPoints, int robotId, - double timeStep) { - auto ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, true, 0, 0); - auto theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field, false, 0, 0); - - for (size_t i = 0; i < pathPoints.size(); i++) { - if (ourDefenseArea.contains(pathPoints[i]) || theirDefenseArea.contains(pathPoints[i])) { - // Don't care about the defense area if the robot is already in the defense area. It should just get out as fast as possible :) - if (i == 0) return; - - insertCollisionData(collisionDatas, CollisionData{pathPoints[i], pathPoints[i], i * timeStep, "DefenseAreaCollision"}); - return; - } - } -} - -void WorldObjects::calculateBallCollisions(const rtt::world::World *world, std::vector &collisionDatas, std::vector pathPoints, double timeStep) { - if (ruleset.minDistanceToBall > 0) { - auto startPositionBall = world->getWorld()->getBall()->get()->getPos(); - auto VelocityBall = world->getWorld()->getBall()->get()->getFilteredVelocity(); - std::vector ballTrajectory; - - // TODO: improve ball trajectory approximation - // Current approximation assumes it continues on the same path with the same velocity, and we check 1 second deep - double time = 0; - double ballAvoidanceTime = 1; - while (pathPoints.size() * timeStep > time && time < ballAvoidanceTime) { - ballTrajectory.emplace_back(startPositionBall + VelocityBall * time); - time += timeStep; - } - - // Check each timeStep for a collision with the ball, or during ball placement if its too close to the 'ballTube' - auto ballTube = LineSegment(startPositionBall, rtt::ai::GameStateManager::getRefereeDesignatedPosition()); - for (size_t i = 0; i < ballTrajectory.size(); i++) { - if (ruleset.minDistanceToBall > (pathPoints[i] - ballTrajectory[i]).length() || - (gameState.getStrategyName() == "ball_placement_them" && ruleset.minDistanceToBall > ballTube.distanceToLine(pathPoints[i]))) { - insertCollisionData(collisionDatas, CollisionData{ballTrajectory[i], pathPoints[i], i * timeStep, "BallCollision"}); - return; - } - } - } -} - -void WorldObjects::calculateEnemyRobotCollisions(const rtt::world::World *world, rtt::Trajectory2D Trajectory, std::vector &collisionDatas, - const std::vector &pathPoints, double timeStep, size_t timeStepsDone) { - const std::vector theirRobots = world->getWorld()->getThem(); - - for (size_t i = timeStepsDone; i < pathPoints.size(); i++) { - double currentTime = i * timeStep; - // The >= 2 is used for checking for collisions within 2 seconds - // TODO: fine tune maximum collision check time - if (currentTime - timeStepsDone * timeStep >= 2) break; - // Vector2 ourVel = Trajectory.getVelocity(currentTime); - for (const auto &theirRobot : theirRobots) { - Vector2 theirVel = theirRobot->getVel(); - // TODO: improve position prediction. Current model uses x + v*t - Vector2 theirPos = theirRobot->getPos() + theirVel * currentTime; - Vector2 posDif = Trajectory.getPosition(currentTime) - theirPos; - // TODO: fine tune avoidance distance - if (posDif.length() < 3 * ai::Constants::ROBOT_RADIUS_MAX()) { - // Vector2 velDif = ourVel - theirVel; - // double projectLength = velDif.dot(posDif) / posDif.length(); - // TODO: fine tune allowed speed difference - // if (abs(projectLength) > 1.5 && theirVel.length() < ourVel.length()) { - insertCollisionData(collisionDatas, CollisionData{theirPos, pathPoints[i], i * timeStep, "EnemyRobotCollision"}); - return; - //} - } - } - } -} - -void WorldObjects::calculateOurRobotCollisions(const rtt::world::World *world, std::vector &collisionDatas, const std::vector &pathPoints, - const std::unordered_map> &computedPaths, int robotId, double timeStep, size_t timeStepsDone) { - auto ourRobots = world->getWorld()->getUs(); - for (size_t i = timeStepsDone; i < pathPoints.size(); i++) { - if (i * timeStep - timeStepsDone * timeStep > 2) break; // Only check for collisions with our robots in the first 2 seconds of our trajectory - for (auto &robot : ourRobots) { - if (robotId != robot->getId()) { - if (computedPaths.find(robot->getId()) != computedPaths.end() && computedPaths.at(robot->getId()).size() > i) { - if ((pathPoints[i] - computedPaths.at(robot->getId())[i]).length() < ai::Constants::ROBOT_RADIUS() * 3 /*1.5*/) { - insertCollisionData(collisionDatas, CollisionData{computedPaths.at(robot->getId())[i], pathPoints[i], i * timeStep, "OurRobotCollision"}); - return; - } - } else { - if ((pathPoints[i] - robot->getPos()).length() < ai::Constants::ROBOT_RADIUS() * 3 /*1.5*/) { - insertCollisionData(collisionDatas, CollisionData{robot->getPos(), pathPoints[i], i * timeStep, "OurRobotCollision"}); - return; - } - } - } - } - } -} - -void WorldObjects::insertCollisionData(std::vector &collisionDatas, const CollisionData &collisionData) { - collisionDatas.insert(std::upper_bound(collisionDatas.begin(), collisionDatas.end(), collisionData, - [](CollisionData const &data, CollisionData const &compare) { return data.collisionTime < compare.collisionTime; }), - collisionData); -} -} // namespace rtt::BB \ No newline at end of file diff --git a/src/control/positionControl/CollisionDetector.cpp b/src/control/positionControl/CollisionDetector.cpp index c30dd50288..7ef3cb1220 100644 --- a/src/control/positionControl/CollisionDetector.cpp +++ b/src/control/positionControl/CollisionDetector.cpp @@ -1,58 +1,159 @@ // -// Created by ratoone on 10-12-19. +// Created by martin on 11-5-22. // #include "control/positionControl/CollisionDetector.h" -#include "control/ControlUtils.h" -#include "world/FieldComputations.h" +#include +#include + +#include "control/positionControl/PositionControlUtils.h" namespace rtt::ai::control { -bool CollisionDetector::isCollisionBetweenPoints(const Vector2& initialPoint, const Vector2& nextPoint) { - bool isFieldColliding = field ? !isPointInsideField(nextPoint) || getDefenseAreaCollision(initialPoint, nextPoint) : false; +template +std::optional CollisionDetector::getFirstObjectCollision(std::span path, int robotId, bool shouldAvoidBall, int timeOffset, int timeLimit) const { + ZoneScopedN("Get Object Collision"); + for (int i = timeOffset; i < path.size() && i < STEP_COUNT && i <= timeLimit; i++) { + auto pathPoint = path[i]; + auto obstacles = timeline[i]; - // colliding with the outside of the field, the defense area, or collision with a robot - return isFieldColliding || getRobotCollisionBetweenPoints(initialPoint, nextPoint); -} + if (shouldAvoidBall && isCollision(pathPoint, obstacles.ball.position, minBallDistance)) { + return {Collision{obstacles.ball.position, obstacles.ball.velocity, i}}; + } + + for (const auto& [otherRobotId, otherRobot] : obstacles.robotsUs) { + if (otherRobotId == robotId) continue; + if (!isCollision(pathPoint, otherRobot.position, MIN_ROBOT_DISTANCE)) continue; + return {Collision{otherRobot.position, otherRobot.velocity, i}}; + } -std::optional CollisionDetector::getCollisionBetweenPoints(const Vector2& point, const Vector2& nextPoint) { - auto robotCollision = getRobotCollisionBetweenPoints(point, nextPoint); - auto defenseCollision = getDefenseAreaCollision(point, nextPoint); + for (const auto& robot : obstacles.robotsThem) { + if (!isCollision(pathPoint, robot.position, MIN_ROBOT_DISTANCE)) continue; + return {Collision{robot.position, robot.velocity, i}}; + } + } - return (defenseCollision.value_or(nextPoint) - point).length2() < (robotCollision.value_or(nextPoint) - point).length2() ? defenseCollision : robotCollision; + return std::nullopt; } -bool CollisionDetector::isPointInsideField(const Vector2& point) { return FieldComputations::pointIsInField(*field, point, Constants::ROBOT_RADIUS()); } +template +std::optional CollisionDetector::getFirstFieldCollision(std::span path) const { + ZoneScopedN("Get Field Collision"); + if (!field.has_value()) return std::nullopt; -std::optional CollisionDetector::getDefenseAreaCollision(const Vector2& point, const Vector2& nextPoint) { - auto ourDefenseCollision = FieldComputations::lineIntersectionWithDefenseArea(*field, true, point, nextPoint, DEFAULT_ROBOT_COLLISION_RADIUS); - if (ourDefenseCollision) { - return *ourDefenseCollision; + for (int i = 0; i < path.size(); i++) { + auto pathPoint = unwrapPosition(path[i]); + if (rtt::ai::FieldComputations::pointIsInField(field.value(), pathPoint, rtt::ai::Constants::ROBOT_RADIUS())) continue; + + // Don't care about the field if the robot is already outside the field (i == 0 is the first point of the robot's path, so almost the currentPosition). + if (i == 0) return std::nullopt; + return Collision{pathPoint, Vector2{0, 0}, i}; } - auto theirDefenseCollision = FieldComputations::lineIntersectionWithDefenseArea(*field, false, point, nextPoint, DEFAULT_ROBOT_COLLISION_RADIUS); - if (!theirDefenseCollision) { - return std::nullopt; + return std::nullopt; +} + +template +std::optional CollisionDetector::getFirstDefenseAreaCollision(std::span path) const { + ZoneScopedN("Get Defense Area Collision"); + if (!field.has_value()) return std::nullopt; + + auto ourDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field.value(), true, 0, 0); + auto theirDefenseArea = rtt::ai::FieldComputations::getDefenseArea(field.value(), false, 0, 0); + for (int i = 0; i < path.size(); i++) { + auto pathPoint = unwrapPosition(path[i]); + if (!ourDefenseArea.contains(pathPoint) && !theirDefenseArea.contains(pathPoint)) continue; + + // Don't care about the defense area if the robot is already in the defense area. It should just get out as fast as possible :) + if (i == 0) return std::nullopt; // ??? + return Collision{pathPoint, Vector2{0, 0}, i}; } - return *theirDefenseCollision; + + return std::nullopt; } -std::optional CollisionDetector::getRobotCollisionBetweenPoints(const Vector2& initialPoint, const Vector2& nextPoint) { - for (const auto& position : robotPositions) { - // if the initial point is already close to a robot, then either 1. there is a collision, or 2. it is the original robot - if ((position - initialPoint).length() > Constants::ROBOT_RADIUS() && LineSegment(initialPoint, nextPoint).distanceToLine(position) < DEFAULT_ROBOT_COLLISION_RADIUS) { - return position; +CollisionDetector::CollisionDetector() { timeline.resize(STEP_COUNT); } + +template +std::optional CollisionDetector::getFirstCollision(std::span path, int robotId, const stp::AvoidObjects& avoidObjects, int timeOffset) const { + ZoneScopedN("Get First Collision"); + // Collision with objects always takes precedence (i.e. happens BEFORE collision with field or defense area) over field collisions. + auto collision = getFirstObjectCollision(path, robotId, avoidObjects.shouldAvoidBall, timeOffset); + if (avoidObjects.shouldAvoidOutOfField && !collision.has_value()) collision = getFirstFieldCollision(path); + if (avoidObjects.shouldAvoidDefenseArea && !collision.has_value()) collision = getFirstDefenseAreaCollision(path); + return collision; +} + +template +bool CollisionDetector::isCollision(const T& pathPoint, const Vector2& obstaclePos, double minDistance) { + auto distance = (unwrapPosition(pathPoint) - obstaclePos).length(); + return distance < minDistance; +} + +void CollisionDetector::updateTimeline(const std::vector& robots, const std::optional& ball) { + for (int i = 0; i < STEP_COUNT; i++) { + auto& positionsAtTime = timeline[i]; + positionsAtTime.robotsThem.clear(); + + double time = i * (TIME_STEP / 1000.0); + for (const auto& robot : robots) { + if (robot->getTeam() == rtt::world::Team::them) { + positionsAtTime.robotsThem.emplace_back(BB::PosVelVector{robot->getPos() + robot->getVel() * time, robot->getVel()}); + continue; + } + + if (!PositionControlUtils::isMoving(robot->getPos()) || i == 0) { + positionsAtTime.robotsUs[robot->getId()] = BB::PosVelVector{robot->getPos(), Vector2{0, 0}}; + } + } + + if (ball.has_value()) { + positionsAtTime.ball = BB::PosVelVector{ball->get()->getPos() + ball->get()->getVelocity() * time, ball->get()->getVelocity()}; + } else { + // TODO: Should we set the ball position to the last known position or somewhere else? + positionsAtTime.ball = BB::PosVelVector{Vector2{0, 0}, Vector2{0, 0}}; } } +} + +void CollisionDetector::setMinBallDistance(double distance) { minBallDistance = distance; } + +void CollisionDetector::setField(const std::optional& newField) { field = newField; } + +void CollisionDetector::updateTimelineForOurRobot(std::span path, const Vector2& currentPosition, int robotId) { + // Sometimes robot did not reach the next step, thus we want to offset the collision by 1. + // timeline[0] is filed with current position at the start of the tick + int offset = !path.empty() && !PositionControlUtils::isTargetReached(path[0].position, currentPosition) ? 1 : 0; + for (int i = offset; i < STEP_COUNT; i++) { + timeline[i].robotsUs[robotId] = i < path.size() ? path[i] : BB::PosVelVector{currentPosition, Vector2{0, 0}}; + } +} - return std::nullopt; +template +bool CollisionDetector::isOccupied(T& position, int robotId, const stp::AvoidObjects& avoidObjects) const { + auto path = std::vector{unwrapPosition(position)}; + auto collision = getFirstObjectCollision(std::span(path), robotId, avoidObjects.shouldAvoidBall, 0, 1); + return collision.has_value() && !PositionControlUtils::isMoving(collision->velocity); } -std::vector CollisionDetector::getRobotPositions() { return robotPositions; } +template +constexpr const Vector2& CollisionDetector::unwrapPosition(const T& container) { + // "if constexpr" and "else" is needed, otherwise compiler is confused about the type of the container and errors out. + // => cannot be reduced to ternary operator + if constexpr (std::is_same_v, BB::PosVelVector>) { + return container.position; + } else { + return container; + } +} -void CollisionDetector::setField(const rtt::world::Field& field_) { this->field = &field_; } +// Explicit instantiation is needed for linking. +template std::optional CollisionDetector::getFirstCollision(std::span, int, const stp::AvoidObjects&, int) const; +template std::optional CollisionDetector::getFirstCollision(std::span, int, const stp::AvoidObjects&, int) const; +template std::optional CollisionDetector::getFirstCollision(std::span, int, const stp::AvoidObjects&, int) const; +template std::optional CollisionDetector::getFirstCollision(std::span, int, const stp::AvoidObjects&, int) const; -void CollisionDetector::setRobotPositions(std::vector& robotPositions_) { this->robotPositions = robotPositions_; } +template bool CollisionDetector::isOccupied(Vector2&, int, const stp::AvoidObjects&) const; -} // namespace rtt::ai::control \ No newline at end of file +} // namespace rtt::ai::control diff --git a/src/control/positionControl/PathPointNode.cpp b/src/control/positionControl/PathPointNode.cpp deleted file mode 100644 index 1d55f9f15a..0000000000 --- a/src/control/positionControl/PathPointNode.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// -// Created by ratoone on 20-02-20. -// - -#include "control/positionControl/PathPointNode.h" - -namespace rtt::ai::control { -PathPointNode::PathPointNode(const Vector2 &position) : position(position) {} - -PathPointNode::PathPointNode(const Vector2 &position, PathPointNode &parent) : position{position}, parent{&parent} {} - -const Vector2 &PathPointNode::getPosition() const { return position; } - -PathPointNode *PathPointNode::getParent() const { return parent; } - -void PathPointNode::setParent(PathPointNode &parent) { this->parent = &parent; } -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/PositionControl.cpp b/src/control/positionControl/PositionControl.cpp index 94bbdb20b8..44634639db 100644 --- a/src/control/positionControl/PositionControl.cpp +++ b/src/control/positionControl/PositionControl.cpp @@ -4,222 +4,41 @@ #include "control/positionControl/PositionControl.h" +#include + #include "control/positionControl/BBTrajectories/BBTrajectory2D.h" -#include "roboteam_utils/Print.h" namespace rtt::ai::control { -RobotCommand PositionControl::computeAndTrackPath(const rtt::world::Field &field, int robotId, const Vector2 ¤tPosition, const Vector2 ¤tVelocity, - Vector2 &targetPosition, stp::PIDType pidType) { - collisionDetector.setField(field); - - // if the robot is close to the final position and can't get there, stop - if ((currentPosition - targetPosition).length() < FINAL_AVOIDANCE_DISTANCE && collisionDetector.getRobotCollisionBetweenPoints(currentPosition, targetPosition)) { - RTT_INFO("Path collides with something close to the target position for robot ID ", robotId) - return {}; - } - if (shouldRecalculatePath(currentPosition, targetPosition, currentVelocity, robotId)) { - computedPaths[robotId] = pathPlanningAlgorithm.computePath(currentPosition, targetPosition); - } - interface::Input::drawData(interface::Visual::PATHFINDING, computedPaths[robotId], Qt::green, robotId, interface::Drawing::LINES_CONNECTED); - interface::Input::drawData(interface::Visual::PATHFINDING, {computedPaths[robotId].front(), currentPosition}, Qt::green, robotId, interface::Drawing::LINES_CONNECTED); - interface::Input::drawData(interface::Visual::PATHFINDING, computedPaths[robotId], Qt::blue, robotId, interface::Drawing::DOTS); - - RobotCommand command = {}; - Position trackingVelocity = pathTrackingAlgorithm.trackPathDefaultAngle(currentPosition, currentVelocity, computedPaths[robotId], robotId, pidType); - command.velocity = Vector2(trackingVelocity.x, trackingVelocity.y); - command.targetAngle = trackingVelocity.rot; - return command; -} - -bool PositionControl::shouldRecalculatePath(const Vector2 ¤tPosition, const Vector2 &targetPos, const Vector2 ¤tVelocity, int robotId) { - return computedPaths[robotId].empty() || PositionControlUtils::isTargetChanged(targetPos, computedPaths[robotId].back()) || - (currentVelocity != Vector2(0, 0) && collisionDetector.isCollisionBetweenPoints(currentPosition, computedPaths[robotId].front())); -} - -void PositionControl::setRobotPositions(std::vector &robotPositions) { collisionDetector.setRobotPositions(robotPositions); } - -rtt::BB::CommandCollision PositionControl::computeAndTrackTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 currentPosition, - Vector2 currentVelocity, Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, - stp::AvoidObjects avoidObjects) { - double timeStep = 0.1; - - std::optional firstCollision; - rtt::BB::CommandCollision commandCollision; - - if (shouldRecalculateTrajectory(world, field, robotId, targetPosition, avoidObjects)) { - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - - // Check path to original target for collisions - firstCollision = worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects); - - if (firstCollision.has_value()) { - if (computedTrajectories[robotId].getTotalTime() - firstCollision->collisionTime > 0.2) { - // RTT_DEBUG(firstCollision->collisionName); - // Create intermediate points, return a collision-free trajectory originating from the best option of these points - auto newTrajectory = - findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, firstCollision, targetPosition, maxRobotVelocity, timeStep, avoidObjects); - if (newTrajectory.has_value()) { - computedTrajectories[robotId] = newTrajectory.value(); - } else { - commandCollision.collisionData = firstCollision; - // RTT_DEBUG("Could not find a collision-free path"); - } - } else { - commandCollision.collisionData = firstCollision; - } - } - - computedPaths[robotId] = computedTrajectories[robotId].getPathApproach(timeStep); - computedPathsVel[robotId] = computedTrajectories[robotId].getVelocityVector(timeStep); // creates a vector with all the velocities - computedPathsPosVel[robotId].clear(); - computedPathsPosVel[robotId].reserve(computedPaths[robotId].size()); - for (size_t i = 0; i < computedPaths[robotId].size(); i++) { - computedPathsPosVel[robotId].push_back(std::make_pair(computedPaths[robotId][i], computedPathsVel[robotId][i])); - } - } - - interface::Input::drawData(interface::Visual::PATHFINDING, computedPaths[robotId], Qt::yellow, robotId, interface::Drawing::LINES_CONNECTED); - interface::Input::drawData(interface::Visual::PATHFINDING, {computedPaths[robotId].front(), currentPosition}, Qt::darkMagenta, robotId, interface::Drawing::LINES_CONNECTED); - interface::Input::drawData(interface::Visual::PATHFINDING, computedPaths[robotId], Qt::magenta, robotId, interface::Drawing::DOTS); - - // Current method is very hacky - // If you are closer to the target than the first point of the approximated path, remove it - if (computedPaths[robotId].size() > 1 && (targetPosition - currentPosition).length() < (targetPosition - computedPaths[robotId].front()).length()) { - computedPaths[robotId].erase(computedPaths[robotId].begin()); - } - - commandCollision.robotCommand = {}; - // Position trackingVelocity = pathTrackingAlgorithm.trackPathDefaultAngle(currentPosition, currentVelocity,computedPaths[robotId], robotId, pidType); - Position trackingVelocity = pathTrackingAlgorithmBBT.trackPathForwardAngle(currentPosition, currentVelocity, computedPathsPosVel[robotId], robotId, pidType); - Vector2 trackingVelocityVector = {trackingVelocity.x, trackingVelocity.y}; - // If there is a collision on the path (so no collision-free path could be found), lower the speed to 1 m/s. This increases the chances of finding a new path - // while also decreasing the speed at which collisions happen - if (commandCollision.collisionData.has_value()) { - if (trackingVelocityVector.length() > 1) trackingVelocityVector = trackingVelocityVector.stretchToLength(1); +PositionControlCommand PositionControl::computeAndTrackTrajectory(const rtt::world::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity, + Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, stp::AvoidObjects avoidObjects) { + auto command = PositionControlCommand{}; + if (collisionDetector.isOccupied(targetPosition, robotId, avoidObjects)) { + command.isOccupied = true; + return command; } - commandCollision.robotCommand.velocity = trackingVelocityVector; - commandCollision.robotCommand.targetAngle = trackingVelocity.rot; - - return commandCollision; -} - -std::optional PositionControl::findNewTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 ¤tPosition, - Vector2 ¤tVelocity, std::optional &firstCollision, Vector2 &targetPosition, - double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { - auto intermediatePoints = createIntermediatePoints(field, robotId, firstCollision, targetPosition); - auto intermediatePointsSorted = scoreIntermediatePoints(intermediatePoints, firstCollision); - - Trajectory2D trajectoryToIntermediatePoint; - while (!intermediatePointsSorted.empty()) { - trajectoryToIntermediatePoint = Trajectory2D(currentPosition, currentVelocity, intermediatePointsSorted.top().second, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - - auto intermediatePathCollision = worldObjects.getFirstCollision(world, field, trajectoryToIntermediatePoint, computedPaths, robotId, avoidObjects); - auto trajectoryAroundCollision = calculateTrajectoryAroundCollision(world, field, intermediatePathCollision, trajectoryToIntermediatePoint, targetPosition, robotId, - maxRobotVelocity, timeStep, avoidObjects); - if (trajectoryAroundCollision.has_value()) { - interface::Input::drawData(interface::Visual::PATHFINDING, intermediatePoints, Qt::green, robotId, interface::Drawing::CROSSES); - interface::Input::drawData(interface::Visual::PATHFINDING, {firstCollision->collisionPosition}, Qt::red, robotId, interface::Drawing::CROSSES); - - interface::Input::drawData(interface::Visual::PATHFINDING, trajectoryToIntermediatePoint.getPathApproach(timeStep), Qt::white, robotId, - interface::Drawing::LINES_CONNECTED); - interface::Input::drawData(interface::Visual::PATHFINDING, trajectoryAroundCollision.value().getPathApproach(timeStep), Qt::yellow, robotId, - interface::Drawing::LINES_CONNECTED); - return trajectoryAroundCollision.value(); - } - intermediatePointsSorted.pop(); + if (!pathControllers.contains(robotId)) { + pathControllers.insert({robotId, {BBTPathPlanning(field.getFieldWidth(), maxRobotVelocity, robotId, collisionDetector), BBTPathTracking(robotId, collisionDetector)}}); } - return std::nullopt; -} - -std::optional PositionControl::calculateTrajectoryAroundCollision(const rtt::world::World *world, const rtt::world::Field &field, - std::optional &intermediatePathCollision, - Trajectory2D trajectoryToIntermediatePoint, Vector2 &targetPosition, int robotId, - double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects) { - Trajectory2D intermediateToTarget; - if (!intermediatePathCollision.has_value()) { - timeStep *= 2; - int numberOfTimeSteps = floor(trajectoryToIntermediatePoint.getTotalTime() / timeStep); - for (int i = 0; i < numberOfTimeSteps; i++) { - Vector2 newStart = trajectoryToIntermediatePoint.getPosition(i * timeStep); - Vector2 newVelocity = trajectoryToIntermediatePoint.getVelocity(i * timeStep); - - intermediateToTarget = Trajectory2D(newStart, newVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto newStartCollisions = worldObjects.getFirstCollision(world, field, intermediateToTarget, computedPaths, robotId, avoidObjects); - - if (newStartCollisions.has_value()) { - continue; - } else { - // Add the second part of the trajectory to a part of the trajectory to the intermediate point - trajectoryToIntermediatePoint.addTrajectory(intermediateToTarget, i * timeStep); - return trajectoryToIntermediatePoint; // This is now the whole path - } - } + auto &[pathPlanning, pathTracking] = pathControllers.at(robotId); + auto shouldUpdate = pathTracking.shouldUpdatePath(currentPosition, targetPosition, avoidObjects); + if (shouldUpdate != DONT_UPDATE) { + pathPlanning.updateConstraints(field, avoidObjects, maxRobotVelocity); + auto path = pathPlanning.generateNewPath(currentPosition, currentVelocity, targetPosition); + interface::Input::drawData(interface::Visual::DEBUG, path, Qt::magenta, robotId, interface::Drawing::CROSSES); + pathTracking.updatePath(std::move(path)); } - return std::nullopt; -} -std::vector PositionControl::createIntermediatePoints(const rtt::world::Field &field, int robotId, std::optional &firstCollision, - Vector2 &targetPosition) { - double angleBetweenIntermediatePoints = M_PI_4 / 2; + auto trackingVelocity = pathTracking.trackPathForwardAngle(currentPosition, currentVelocity, pidType); + collisionDetector.updateTimelineForOurRobot(pathTracking.getRemainingPath(), currentPosition, robotId); - // Radius and point extension of intermediate points are based on the fieldWith - auto fieldWidth = field.getFieldWidth(); - - // PointToDrawFrom is picked by drawing a line from the target position to the obstacle and extending that - // line further towards our currentPosition by extension meters. - float pointExtension = fieldWidth / 18; // How far the pointToDrawFrom has to be from the obstaclePosition - Vector2 pointToDrawFrom = firstCollision->obstaclePosition + (firstCollision->obstaclePosition - targetPosition).normalize() * pointExtension; - - std::vector intermediatePoints; - for (int i = -4; i < 5; i++) { - if (i != 0) { - // Make half circle of intermediatePoints pointed towards obstaclePosition, originating from pointToDrawFrom, by rotating pointToRotate with a radius - // intermediatePointRadius - float intermediatePointRadius = fieldWidth / 4; // Radius of the half circle - Vector2 pointToRotate = pointToDrawFrom + (targetPosition - firstCollision->obstaclePosition).normalize() * intermediatePointRadius; - Vector2 intermediatePoint = pointToRotate.rotateAroundPoint(i * angleBetweenIntermediatePoints, pointToDrawFrom); - - /*//If not in a defense area (only checked if robot is not allowed in defense area) - if (worldObjects.canEnterDefenseArea(robotId) || - (!rtt::ai::FieldComputations::pointIsInDefenseArea(field, intermediatePoint, true, 0) && - !rtt::ai::FieldComputations::pointIsInDefenseArea(field, intermediatePoint, false, - 0.2 + rtt::ai::Constants::ROBOT_RADIUS()))) { - //.. and inside the field (only checked if the robot is not allowed outside the field), add this cross to the list - if (worldObjects.canMoveOutsideField(robotId) || - rtt::ai::FieldComputations::pointIsInField(field, intermediatePoint, - rtt::ai::Constants::ROBOT_RADIUS())) {*/ - intermediatePoints.emplace_back(intermediatePoint); - /*} - }*/ - } - } - return intermediatePoints; -} - -std::priority_queue, std::vector>, std::greater<>> PositionControl::scoreIntermediatePoints( - std::vector &intermediatePoints, std::optional &firstCollision) { - double intermediatePointScore; - std::priority_queue, std::vector>, std::greater<>> intermediatePointsSorted; - for (const auto &i : intermediatePoints) { - intermediatePointScore = (i - firstCollision->collisionPosition).length(); - std::pair p = {intermediatePointScore, i}; - intermediatePointsSorted.push(p); - } - return intermediatePointsSorted; + command.robotCommand.velocity = {trackingVelocity.x, trackingVelocity.y}; + command.robotCommand.targetAngle = trackingVelocity.rot; + return command; } -bool PositionControl::shouldRecalculateTrajectory(const rtt::world::World *world, const rtt::world::Field &field, int robotId, Vector2 targetPosition, - ai::stp::AvoidObjects avoidObjects) { - if (!computedTrajectories.contains(robotId) || - (computedPaths.contains(robotId) && !computedPaths[robotId].empty() && - (targetPosition - computedPaths[robotId][computedPaths[robotId].size() - 1]).length() > stp::control_constants::GO_TO_POS_ERROR_MARGIN) || - worldObjects.getFirstCollision(world, field, computedTrajectories[robotId], computedPaths, robotId, avoidObjects).has_value()) { - return true; - } - return false; -} +CollisionDetector &PositionControl::getCollisionDetector() { return collisionDetector; } } // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/PositionControlUtils.cpp b/src/control/positionControl/PositionControlUtils.cpp index 2db79365e6..2069429810 100644 --- a/src/control/positionControl/PositionControlUtils.cpp +++ b/src/control/positionControl/PositionControlUtils.cpp @@ -4,17 +4,16 @@ #include "control/positionControl/PositionControlUtils.h" -namespace rtt::ai::control { +#include "utilities/Constants.h" -bool PositionControlUtils::isTargetChanged(const Vector2 &targetPos, const Vector2 &oldTarget) { return (targetPos - oldTarget).length() > MAX_TARGET_DEVIATION; } +namespace rtt::ai::control { -bool PositionControlUtils::isTargetReached(const Vector2 &targetPos, const Vector2 ¤tPosition) { - return (targetPos - currentPosition).length() < MIN_DISTANCE_TARGET_REACHED; +bool PositionControlUtils::isTargetChanged(const Vector2 &targetPos, const Vector2 &oldTarget) { + return (targetPos - oldTarget).length() > Constants::POSITION_CONTROL_MAX_TARGET_DEVIATION(); } -void PositionControlUtils::removeFirstIfReached(std::vector &path, const Vector2 ¤tPosition) { - if (path.size() > 1 && isTargetReached(path.front(), currentPosition)) { - path.erase(path.begin()); - } +bool PositionControlUtils::isTargetReached(const Vector2 &targetPos, const Vector2 ¤tPosition) { + return (targetPos - currentPosition).length() < Constants::POSITION_CONTROL_MIN_DISTANCE_REACHED(); } +bool PositionControlUtils::isMoving(const Vector2 &velocity) { return velocity.length() > Constants::POSITION_CONTROL_MAX_STILL_VELOCITY(); } } // namespace rtt::ai::control diff --git a/src/control/positionControl/pathPlanning/BBTPathPlanning.cpp b/src/control/positionControl/pathPlanning/BBTPathPlanning.cpp new file mode 100644 index 0000000000..6130d8402b --- /dev/null +++ b/src/control/positionControl/pathPlanning/BBTPathPlanning.cpp @@ -0,0 +1,111 @@ +// +// Created by martin on 14-5-22. +// + +#include "control/positionControl/pathPlanning/BBTPathPlanning.h" + +#include +#include + +#include "control/positionControl/BBTrajectories/BBTrajectory2D.h" +#include "interface/widgets/widget.h" + +namespace rtt::ai::control { + +BBTPathPlanning::BBTPathPlanning(double fieldWidth, double maxVelocity, int robotId, const CollisionDetector& collisionDetector) + : robotId(robotId), collisionDetector(collisionDetector), fieldWidth(fieldWidth), maxVelocity(maxVelocity) {} + +std::vector BBTPathPlanning::generateNewPath(const Vector2& initialPos, const Vector2& initialVel, const Vector2& targetPos) const { + ZoneScopedN("Generate New Path"); + auto trajectory = BB::BBTrajectory2D(initialPos, initialVel, targetPos, maxVelocity, ai::Constants::MAX_ACC_UPPER()); + auto posVel = trajectory.getPosVelVector(); + + auto firstCollision = collisionDetector.getFirstCollision(std::span(posVel), robotId, avoidObjects); + if (!firstCollision.has_value()) return trajectory.getPosVelVector(); + + const auto intermediatePoints = generateIntermediatePoints(initialPos); + auto bestTrajectory = CombinedTrajectory{}; + + for (const auto& intermediatePoint : intermediatePoints) { + auto newTrajectory = bestTrajectoryForIntermediatePoint(initialPos, initialVel, intermediatePoint, targetPos); + interface::Input::drawData(interface::Visual::PATHFINDING_DEBUG, newTrajectory.endTrajectory.getPathApproach(), Qt::red, robotId, interface::Drawing::LINES_CONNECTED); + if (bestTrajectory.score < newTrajectory.score) continue; + bestTrajectory = std::move(newTrajectory); + } + + interface::Input::drawData(interface::Visual::PATHFINDING_DEBUG, intermediatePoints, Qt::yellow, robotId, interface::Drawing::CROSSES); + return extractPath(std::move(bestTrajectory)); +} + +std::vector BBTPathPlanning::extractPath(CombinedTrajectory&& combineTraj) const { + ZoneScopedN("Extract Path"); + auto path = std::vector(); + const auto initialPosVel = combineTraj.initialTrajectory.getPosVelVector(); + const auto endPosVel = combineTraj.endTrajectory.getPosVelVector(); + + path.reserve(combineTraj.cutoffIndex + endPosVel.size()); + path.insert(path.end(), std::make_move_iterator(initialPosVel.begin()), std::make_move_iterator(std::next(initialPosVel.begin(), combineTraj.cutoffIndex))); + path.insert(path.end(), std::make_move_iterator(endPosVel.begin()), std::make_move_iterator(endPosVel.end())); + return path; +} + +std::vector BBTPathPlanning::generateIntermediatePoints(const Vector2& center) const { + ZoneScopedN("Generate Intermediate Points"); + const auto circlesRadius = {fieldWidth / 8, fieldWidth / 4, fieldWidth / 2}; + auto intermediatePoints = std::vector(); + intermediatePoints.reserve(INTERMEDIATE_POINTS_PER_CIRCLE * circlesRadius.size()); + + for (auto radius : circlesRadius) { + auto pointToRotate = center + Vector2{0, radius}; + for (int i = 0; i < INTERMEDIATE_POINTS_PER_CIRCLE; i++) { + intermediatePoints.emplace_back(pointToRotate.rotateAroundPoint(i * ANGLE_BETWEEN_INTERMEDIATE_POINTS, center)); + } + } + + return intermediatePoints; +} + +void BBTPathPlanning::updateConstraints(const rtt::world::Field& field, const stp::AvoidObjects newAvoidObjects, double newMaxVelocity) { + maxVelocity = newMaxVelocity; + fieldWidth = field.getFieldWidth(); + avoidObjects = newAvoidObjects; +} + +CombinedTrajectory BBTPathPlanning::bestTrajectoryForIntermediatePoint(const Vector2& initialPos, const Vector2& initialVel, const Vector2& intermediatePoint, + const Vector2& targetPos) const { + ZoneScopedN("Find Best Trajectory"); + auto bestTrajectory = CombinedTrajectory{BB::BBTrajectory2D(initialPos, initialVel, intermediatePoint, maxVelocity, ai::Constants::MAX_ACC_UPPER())}; + + const auto initialPosVel = bestTrajectory.initialTrajectory.getPosVelVector(); + // Checking each step is not necessary and takes too much time + for (int i = 0; i < initialPosVel.size(); i += 2) { + ZoneScopedN("Optimize Trajectory"); + const auto& cutoffPoint = initialPosVel[i]; + auto endTrajectory = BB::BBTrajectory2D(cutoffPoint.position, cutoffPoint.velocity, targetPos, maxVelocity, ai::Constants::MAX_ACC_UPPER()); + const auto& endPath = endTrajectory.getPathApproach(); + + auto collision = collisionDetector.getFirstCollision(std::span(endPath), robotId, avoidObjects, i); + if (collision.has_value() && collision->timeStep <= i) break; + + int newScore = scorePath(i + endPath.size(), collision); + if (newScore >= bestTrajectory.score) continue; + + bestTrajectory.endTrajectory = std::move(endTrajectory); + bestTrajectory.cutoffIndex = i; + bestTrajectory.score = newScore; + } + + return bestTrajectory; +} + +int BBTPathPlanning::scorePath(int duration, std::optional collision) { + auto score = duration; + if (collision.has_value()) { + score += 500; // Fixed penalty for collision + score -= collision->timeStep; + } + + return score; +} + +} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathPlanning/NumTreesPlanning.cpp b/src/control/positionControl/pathPlanning/NumTreesPlanning.cpp deleted file mode 100644 index daba310a2e..0000000000 --- a/src/control/positionControl/pathPlanning/NumTreesPlanning.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// -// Created by ratoone on 20-02-20. -// - -#include "control/positionControl/pathPlanning/NumTreesPlanning.h" - -namespace rtt::ai::control { -NumTreesPlanning::NumTreesPlanning(CollisionDetector &collisionDetector) : PathPlanningAlgorithm(collisionDetector) {} - -std::vector NumTreesPlanning::computePath(const Vector2 &robotPosition, Vector2 &targetPosition) { - auto root = PathPointNode(robotPosition); - std::queue pointQueue; - pointQueue.push(root); - auto finalPath = root; - int iterations = 0; - - while (!pointQueue.empty()) { - PathPointNode &point = pointQueue.front(); - pointQueue.pop(); - iterations++; - - // the algorithm will branch too much if it can't find a proper path; move anyway and try again later - if (pointQueue.size() >= MAX_BRANCHING || iterations > MAX_ITERATIONS) { - break; - } - - if ((point.getPosition() - targetPosition).length() < TARGET_THRESHOLD) { - finalPath = point; - break; - } - - if (point.getParent()) { - if (!collisionDetector.isPointInsideField(point.getPosition())) { - continue; - } - auto parentCollision = collisionDetector.getCollisionBetweenPoints(point.getParent()->getPosition(), point.getPosition()); - if (parentCollision) { - auto branches = branchPath(*point.getParent(), parentCollision.value(), targetPosition); - std::for_each(branches.begin(), branches.end(), [&pointQueue](PathPointNode &newPoint) { pointQueue.push(newPoint); }); - continue; - } - // current path has no collisions - update current best path - if (point.getPosition() - targetPosition < finalPath.getPosition() - targetPosition) { - finalPath = point; - } - } - - auto collision = collisionDetector.getCollisionBetweenPoints(point.getPosition(), targetPosition); - - // no initial collision - if (!collision) { - finalPath = PathPointNode(targetPosition, point); - break; - } - - auto branches = branchPath(point, collision.value(), targetPosition); - std::for_each(branches.begin(), branches.end(), [&pointQueue](const PathPointNode &newPoint) { pointQueue.push(newPoint); }); - } - - std::vector path{finalPath.getPosition()}; - for (PathPointNode &it = finalPath; it.getParent(); it = *(it.getParent())) { - path.push_back(it.getParent()->getPosition()); - } - std::reverse(path.begin(), path.end()); - // path should contain final point - if there are collisions between the last 2 points, the path will be recalculated - // closer to the destination - if (path.back() != targetPosition) { - path.push_back(targetPosition); - } - return path; -} - -std::vector NumTreesPlanning::branchPath(PathPointNode &parentPoint, const Vector2 &collisionPosition, Vector2 &destination) const { - // const rtt_world::Field &field, const Vector2 &point, bool isOurDefenceArea, double margin, double backMargin - if (collisionPosition.x > 1.9 && abs(collisionPosition.y) < 1.1) { - destination = (destination - Vector2(4, 0)).stretchToLength(0.5) + destination; - } - Vector2 deltaPosition = collisionPosition - parentPoint.getPosition(); - - Vector2 leftTargetPosition = collisionPosition + Vector2(deltaPosition.y, -deltaPosition.x).stretchToLength(AVOIDANCE_DISTANCE); - Vector2 rightTargetPosition = collisionPosition + Vector2(deltaPosition.y, -deltaPosition.x).stretchToLength(-AVOIDANCE_DISTANCE); - - // pick the closest branch to try first - if (leftTargetPosition - destination < rightTargetPosition - destination) { - return {PathPointNode(leftTargetPosition, parentPoint), PathPointNode(rightTargetPosition, parentPoint)}; - } - return {PathPointNode(rightTargetPosition, parentPoint), PathPointNode(leftTargetPosition, parentPoint)}; -} -} // namespace rtt::ai::control diff --git a/src/control/positionControl/pathPlanning/PathPlanningAlgorithm.cpp b/src/control/positionControl/pathPlanning/PathPlanningAlgorithm.cpp deleted file mode 100644 index ea4e00be54..0000000000 --- a/src/control/positionControl/pathPlanning/PathPlanningAlgorithm.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// -// Created by ratoone on 09-03-20. -// - -#include "control/positionControl/pathPlanning/PathPlanningAlgorithm.h" - -namespace rtt::ai::control { -PathPlanningAlgorithm::PathPlanningAlgorithm(CollisionDetector &collisionDetector) : collisionDetector{collisionDetector} {} -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathPlanning/VoronoiPathPlanning.cpp b/src/control/positionControl/pathPlanning/VoronoiPathPlanning.cpp deleted file mode 100644 index 6f226ad8d2..0000000000 --- a/src/control/positionControl/pathPlanning/VoronoiPathPlanning.cpp +++ /dev/null @@ -1,131 +0,0 @@ -// -// Created by ratoone on 05-11-19. -// - -#include "control/positionControl/pathPlanning/VoronoiPathPlanning.h" - -namespace rtt::ai::control { - -VoronoiPathPlanning::VoronoiPathPlanning(CollisionDetector& collisionDetector) : collisionDetector(collisionDetector) {} - -void VoronoiPathPlanning::computeDiagram(const Vector2& robotPosition, const Vector2& targetPosition) { - auto robots = collisionDetector.getRobotPositions(); - std::vector robotPositions(robots.size()); - std::transform(robots.begin(), robots.end(), robotPositions.begin(), [](auto robot) -> jcv_point { return {(float)robot.x, (float)robot.y}; }); - jcv_rect playArea = {(jcv_point){(float)std::min(robotPosition.x, targetPosition.x), (float)std::min(robotPosition.y, targetPosition.y)}, - (jcv_point){(float)std::max(robotPosition.x, targetPosition.x), (float)std::max(robotPosition.y, targetPosition.y)}}; - jcv_diagram_generate(robots.size(), robotPositions.data(), &playArea, nullptr, &voronoiDiagram); -} - -void VoronoiPathPlanning::generateGraphFromDiagram() { - graphAdjacencyList.clear(); - for (const jcv_edge* edgeIterator = jcv_diagram_get_edges(&voronoiDiagram); edgeIterator != nullptr; edgeIterator = edgeIterator->next) { - // ignore edges of the diagram - if (edgeIterator->a == 0 && edgeIterator->b == 0 && edgeIterator->c == 0) { - continue; - } - - double distance = Vector2(edgeIterator->pos[0].x - edgeIterator->pos[1].x, edgeIterator->pos[0].y - edgeIterator->pos[1].y).length(); - - // ignore degenerate lines - if (distance == 0) { - continue; - } - - Vector2 firstEdgePoint = convertFromJcvPoint(edgeIterator->pos[0]); - Vector2 secondEdgePoint = convertFromJcvPoint(edgeIterator->pos[1]); - - // if the vertex is a new one (not found in the map), add it - if (graphAdjacencyList.empty() || graphAdjacencyList.find(firstEdgePoint) == graphAdjacencyList.end()) { - graphAdjacencyList[firstEdgePoint] = std::list(1, (GraphNode){secondEdgePoint, distance}); - } - // else append the adjacency list with the new neighbouring site and the new node connection - else { - graphAdjacencyList[firstEdgePoint].push_back((GraphNode){secondEdgePoint, distance}); - } - - // repeat for the second node - if (graphAdjacencyList.empty() || graphAdjacencyList.find(secondEdgePoint) == graphAdjacencyList.end()) { - graphAdjacencyList[secondEdgePoint] = std::list(1, (GraphNode){firstEdgePoint, distance}); - } else { - graphAdjacencyList[secondEdgePoint].push_back((GraphNode){firstEdgePoint, distance}); - } - } -} - -void VoronoiPathPlanning::addPointToGraph(const Vector2& pointToAdd) { - // 1 or fewer obstacles in the area - no points in the graph - if (graphAdjacencyList.empty()) { - graphAdjacencyList[pointToAdd] = {}; - return; - } - - double minDist = -1; - Vector2 closestPoint; - // add point to the graph, connecting it to the closest node - for (auto const& positionIterator : graphAdjacencyList) { - if (minDist < 0 || (positionIterator.first - pointToAdd).length() < minDist) { - closestPoint = positionIterator.first; - minDist = (positionIterator.first - pointToAdd).length(); - } - } - if (graphAdjacencyList.find(pointToAdd) == graphAdjacencyList.end()) { - graphAdjacencyList[pointToAdd] = std::list(1, (GraphNode){closestPoint, minDist}); - graphAdjacencyList[closestPoint].push_back({pointToAdd, minDist}); - } -} - -std::vector VoronoiPathPlanning::generatePathDijkstra(const Vector2& initialPosition, const Vector2& targetPosition) { - std::unordered_map distanceVector; - std::unordered_map parentVector; - distanceVector[initialPosition] = 0; - std::list nodeQueue; - nodeQueue.push_front(initialPosition); - for (const Vector2& currentNode : nodeQueue) { - if (currentNode == targetPosition) { - break; - } - // for each node in the queue, check its neighbors - for (const GraphNode& adjacentNode : graphAdjacencyList[currentNode]) { - if (distanceVector.find(adjacentNode.nextNodePosition) == distanceVector.end()) { - // check if it's already visited or queued - if (std::find(nodeQueue.begin(), nodeQueue.end(), adjacentNode.nextNodePosition) == nodeQueue.end()) { - nodeQueue.push_back(adjacentNode.nextNodePosition); - } - distanceVector[adjacentNode.nextNodePosition] = distanceVector[currentNode] + adjacentNode.distance; - parentVector[adjacentNode.nextNodePosition] = currentNode; - continue; - } - // if the current node's traversal causes a shorter path to get to adjacentNode, update its parent and cost - if (distanceVector[adjacentNode.nextNodePosition] > distanceVector[currentNode] + adjacentNode.distance) { - distanceVector[adjacentNode.nextNodePosition] = distanceVector[currentNode] + adjacentNode.distance; - parentVector[adjacentNode.nextNodePosition] = currentNode; - } - } - } - - std::vector pathPoints; - for (Vector2 backtrack = targetPosition; parentVector.find(backtrack) != parentVector.end(); backtrack = parentVector[backtrack]) { - pathPoints.push_back(backtrack); - } - - // as older points get pushed back, the vector has to be reversed - std::reverse(pathPoints.begin(), pathPoints.end()); - return pathPoints; -} - -std::vector VoronoiPathPlanning::computePath(const Vector2& robotPosition, const Vector2& targetPosition) { - computeDiagram(robotPosition, targetPosition); - generateGraphFromDiagram(); - // TODO: avoid one obstacle in voronoi - addPointToGraph(robotPosition); - addPointToGraph(targetPosition); - return generatePathDijkstra(robotPosition, targetPosition); -} - -Vector2 VoronoiPathPlanning::convertFromJcvPoint(jcv_point point) { - // rounded to 3 decimals for testing purposes - 1mm precision is enough - return Vector2(std::round(1000.0 * point.x) / 1000, std::round(1000.0 * point.y) / 1000); -} - -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathTracking/BBTPathTracking.cpp b/src/control/positionControl/pathTracking/BBTPathTracking.cpp index 49e6ced715..cf28dbfb32 100644 --- a/src/control/positionControl/pathTracking/BBTPathTracking.cpp +++ b/src/control/positionControl/pathTracking/BBTPathTracking.cpp @@ -6,27 +6,65 @@ #include +#include + #include "roboteam_utils/Print.h" namespace rtt::ai::control { -Position BBTPathTracking::trackPathForwardAngle(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector> &pathVelocityPoints, - int robotId, stp::PIDType pidType) { - int lookAhead = std::min(pathVelocityPoints.size(), STEPS_AHEAD); - Vector2 currentTarget = std::next(pathVelocityPoints.begin(), lookAhead - 1)->first; - Vector2 currentTargetVelocity = std::next(pathVelocityPoints.begin(), lookAhead - 1)->second; - if (pathVelocityPoints.size() > 1 && PositionControlUtils::isTargetReached(currentTarget, currentPosition)) { - // track the Nth point, or the last if the size is smaller than N; the untracked ones are discarded - pathVelocityPoints.erase(pathVelocityPoints.begin(), std::next(pathVelocityPoints.begin(), lookAhead)); +Position BBTPathTracking::trackPathForwardAngle(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, stp::PIDType pidType) { + interface::Input::drawData(interface::Visual::PATHFINDING, remainingPath, Qt::yellow, robotId, interface::Drawing::LINES_CONNECTED); + interface::Input::drawData(interface::Visual::PATHFINDING, remainingPath, Qt::green, robotId, interface::Drawing::DOTS); + + if (remainingPath.size() > ai::Constants::POSITION_CONTROL_STEP_COUNT()) { + interface::Input::drawData(interface::Visual::PATHFINDING, {remainingPath[ai::Constants::POSITION_CONTROL_STEP_COUNT()].position}, Qt::red, robotId, + interface::Drawing::DOTS); + } + + if (remainingPath.empty()) { + return {0, 0, 0}; } - std::vector tempPath{currentTarget}; - Position pidPosition = pidTracking.trackPath(currentPosition, currentVelocity, tempPath, robotId, 0, pidType); - Vector2 pidVelocity{pidPosition.x, pidPosition.y}; - if (pathVelocityPoints.size() > 2) { - pidVelocity = pidVelocity.stretchToLength(currentTargetVelocity.length()); + auto lookAhead = std::min(remainingPath.size(), STEPS_AHEAD); + const auto ¤tTarget = std::next(remainingPath.begin(), lookAhead - 1); + if (PositionControlUtils::isTargetReached(currentTarget->position, currentPosition)) { + // Track the Nth point, or the last if the size is smaller than N; the untracked ones are discarded + remainingPath = remainingPath.subspan(lookAhead); } - Position returnPosition{pidVelocity.x, pidVelocity.y, (currentTarget - currentPosition).angle()}; - return returnPosition; + + const auto newPid = PositionControlUtils::getPIDValue(pidType); + xPID.setPID(newPid); + yPID.setPID(newPid); + + auto pidVelocity = + Vector2{ + xPID.getOutput(currentPosition.x, currentTarget->position.x), + yPID.getOutput(currentPosition.y, currentTarget->position.y), + } + .stretchToLength(currentTarget->velocity.length()); + + return {pidVelocity.x, pidVelocity.y, (currentTarget->position - currentPosition).angle()}; +} + +UpdatePath BBTPathTracking::shouldUpdatePath(const Vector2 ¤tPos, const Vector2 &targetPos, const stp::AvoidObjects &avoidObjects) { + if (!remainingPath.empty() && PositionControlUtils::isTargetChanged(targetPos, remainingPath.back().position)) return UPDATE_TARGET_CHANGED; + if (remainingPath.empty() && !PositionControlUtils::isTargetReached(targetPos, currentPos)) return UPDATE_TARGET_REACHED; + + auto collision = collisionDetector.getFirstCollision(remainingPath, robotId, avoidObjects); + if (collision.has_value() && remainingPath.size() > 2) return UPDATE_COLLISION_DETECTED; + + return DONT_UPDATE; +} + +void BBTPathTracking::updatePath(std::vector &&newPath) { + path = std::move(newPath); + remainingPath = std::span(path); +} +BBTPathTracking::BBTPathTracking(int robotId, const CollisionDetector &collisionDetector) : robotId(robotId), collisionDetector(collisionDetector) { + xPID.setMaxIOutput(Constants::MAX_VEL()); + yPID.setMaxIOutput(Constants::MAX_VEL()); } + +std::span BBTPathTracking::getRemainingPath() { return remainingPath; } + } // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathTracking/DensePathTracking.cpp b/src/control/positionControl/pathTracking/DensePathTracking.cpp deleted file mode 100644 index c692d51d80..0000000000 --- a/src/control/positionControl/pathTracking/DensePathTracking.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// -// Created by ratoone on 24-01-20. -// - -#include "control/positionControl/pathTracking/DensePathTracking.h" - -#include "control/positionControl/PositionControlUtils.h" - -namespace rtt::ai::control { - -Position DensePathTracking::trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, double angle, - stp::PIDType pidType) { - int lookAhead = std::min(pathPoints.size(), STEPS_AHEAD); - Vector2 currentTarget = *std::next(pathPoints.begin(), lookAhead - 1); - if (pathPoints.size() > 1 && PositionControlUtils::isTargetReached(currentTarget, currentPosition)) { - // track the Nth point, or the last if the size is smaller than N; the untracked ones are discarded - pathPoints.erase(pathPoints.begin(), std::next(pathPoints.begin(), lookAhead)); - } - - std::vector tempPath{currentTarget}; - return pidTracking.trackPath(currentPosition, currentVelocity, tempPath, robotId, angle, pidType); -} -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathTracking/PathTrackingAlgorithm.cpp b/src/control/positionControl/pathTracking/PathTrackingAlgorithm.cpp deleted file mode 100644 index 7aa9ead297..0000000000 --- a/src/control/positionControl/pathTracking/PathTrackingAlgorithm.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by ratoone on 09-03-20. -// - -#include "control/positionControl/pathTracking/PathTrackingAlgorithm.h" - -namespace rtt::ai::control { -Position control::PathTrackingAlgorithm::trackPathDefaultAngle(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, std::vector &pathPoints, int robotId, - stp::PIDType pidType) { - auto position = trackPath(currentPosition, currentVelocity, pathPoints, robotId, 0, pidType); - Vector2 velocity = {position.x, position.y}; - return {velocity, (pathPoints.front() - currentPosition).angle()}; -} -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/control/positionControl/pathTracking/PidTracking.cpp b/src/control/positionControl/pathTracking/PidTracking.cpp deleted file mode 100644 index d211041c29..0000000000 --- a/src/control/positionControl/pathTracking/PidTracking.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// -// Created by ratoone on 24-01-20. -// - -#include "control/positionControl/pathTracking/PidTracking.h" - -namespace rtt::ai::control { - -Position PidTracking::trackPath(const Vector2 ¤tPosition, const Vector2 ¤tVelocity, 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()); - pidMapping[robotId].first.setMaxIOutput(MAX_VELOCITY); - pidMapping[robotId].second.setMaxIOutput(MAX_VELOCITY); - } - - updatePIDValues(pidType, robotId); - - Vector2 velocity{}; - velocity.x = pidMapping[robotId].first.getOutput(currentPosition.x, pathPoints.front().x); - velocity.y = pidMapping[robotId].second.getOutput(currentPosition.y, pathPoints.front().y); - - return {velocity, angle}; -} - -void PidTracking::updatePidValuesFromInterface(bool isKeeper) { - auto newPid = isKeeper ? interface::Output::getKeeperPid() : interface::Output::getNumTreePid(); - for (auto pid : pidMapping) { - pidMapping[pid.first].first.setPID(newPid); - pidMapping[pid.first].second.setPID(newPid); - } -} - -void PidTracking::updatePIDValues(stp::PIDType pidType, int robotID) { - std::tuple newPID; - - switch (pidType) { - case stp::PIDType::DEFAULT: { - newPID = interface::Output::getNumTreePid(); - break; - } - case stp::PIDType::RECEIVE: { - newPID = interface::Output::getReceivePid(); - break; - } - case stp::PIDType::INTERCEPT: { - newPID = interface::Output::getInterceptPid(); - } - case stp::PIDType::KEEPER: { - newPID = interface::Output::getKeeperPid(); - break; - } - case stp::PIDType::KEEPER_INTERCEPT: { - newPID = interface::Output::getKeeperInterceptPid(); - break; - } - } - - pidMapping[robotID].first.setPID(newPID); - pidMapping[robotID].second.setPID(newPID); -} -} // namespace rtt::ai::control \ No newline at end of file diff --git a/src/interface/api/Input.cpp b/src/interface/api/Input.cpp index 20eb13ee56..f368beb106 100644 --- a/src/interface/api/Input.cpp +++ b/src/interface/api/Input.cpp @@ -26,6 +26,15 @@ void Input::drawData(Visual visual, std::vector points, QColor color, i drawings.emplace_back(visual, std::move(points), std::move(color), robotId, method, width, height, strokeWidth); } +void Input::drawData(Visual visual, std::span points, QColor color, int robotId, Drawing::DrawingMethod method, double width, double height, + double strokeWidth) { + auto pathVectors = std::vector(); + pathVectors.reserve(points.size()); + + std::transform(points.begin(), points.end(), std::back_inserter(pathVectors), [](const BB::PosVelVector &pathPoint) { return pathPoint.position; }); + drawData(visual, std::move(pathVectors), std::move(color), robotId, method, width, height, strokeWidth); +} + /* * Useful for debugging: quickly draw a vector of points. */ @@ -36,6 +45,8 @@ void Input::drawDebugData(std::vector points, QColor color, int robotId const std::vector Input::getDrawings() { std::lock_guard lock(drawingMutex); + // auto vectorCopy = std::move(drawings); + // drawings = {}; return drawings; } diff --git a/src/interface/widgets/widget.cpp b/src/interface/widgets/widget.cpp index 2431f46cde..6cf6474310 100644 --- a/src/interface/widgets/widget.cpp +++ b/src/interface/widgets/widget.cpp @@ -18,37 +18,42 @@ Visualizer::Visualizer(QWidget *parent) : QWidget(parent) {} /// The update loop of the field widget. Invoked by widget->update(); void Visualizer::paintEvent(QPaintEvent *event) { + std::chrono::steady_clock::time_point tStart = std::chrono::steady_clock::now(); + QPainter painter(this); painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::HighQualityAntialiasing, true); - std::optional world; - std::optional field; - auto const &[_, worldPtr] = rtt::world::World::instance(); - world = worldPtr->getWorld(); - field = worldPtr->getField(); - - if (!world.has_value() || !world.value()->weHaveRobots()) { - painter.drawText(24, 24, "Waiting for incoming world state"); - return; - } - if (field.has_value()) { - calculateFieldSizeFactor(field.value()); - drawBackground(painter); - drawFieldHints(field.value(), painter); - drawFieldLines(field.value(), painter); - } + { + auto const &[_, worldPtr] = rtt::world::World::instance(); + auto const &world = worldPtr->getWorld(); + auto const &field = worldPtr->getField(); + + if (!world.has_value() || !world.value()->weHaveRobots()) { + painter.drawText(24, 24, "Waiting for incoming world state"); + return; + } + + if (field.has_value()) { + calculateFieldSizeFactor(field.value()); + drawBackground(painter); + drawFieldHints(field.value(), painter); + drawFieldLines(field.value(), painter); + } - auto s = QString::fromStdString("We have " + std::to_string(world->getUs().size()) + " robots"); - painter.drawText(24, 48, s.fromStdString("We have " + std::to_string(world->getUs().size()) + " robots")); + auto s = QString::fromStdString("We have " + std::to_string(world->getUs().size()) + " robots"); + painter.drawText(24, 48, s.fromStdString("We have " + std::to_string(world->getUs().size()) + " robots")); - if (showWorld) { - drawRobots(painter, world.value()); - if (world->getBall().has_value()) drawBall(painter, world->getBall().value()); + if (showWorld) { + drawRobots(painter, world.value()); + if (world->getBall().has_value()) drawBall(painter, world->getBall().value()); + } } // draw the drawings from the input auto drawings = Input::getDrawings(); + Input::clearDrawings(); + // RTT_WARNING("Drawing size: ", drawings.size()); for (auto const &drawing : drawings) { if (!drawing.points.empty()) { bool shouldShow = false; @@ -104,7 +109,7 @@ void Visualizer::paintEvent(QPaintEvent *event) { } } - Input::clearDrawings(); + // Input::clearDrawings(); if (showBallPlacementMarker) drawBallPlacementTarget(painter); @@ -128,6 +133,10 @@ void Visualizer::paintEvent(QPaintEvent *event) { if (showWorldDetections) { drawRawDetectionPackets(painter); } + + std::chrono::steady_clock::time_point tStop = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast((tStop - tStart)).count(); + // RTT_WARNING("Drawing Time: ", duration) } bool Visualizer::shouldVisualize(Toggle toggle, int robotId) { diff --git a/src/stp/Play.cpp b/src/stp/Play.cpp index f90c3ba42c..0f1c501389 100644 --- a/src/stp/Play.cpp +++ b/src/stp/Play.cpp @@ -6,6 +6,7 @@ #include "control/ControlUtils.h" #include "interface/widgets/MainControlsWidget.h" +#include namespace rtt::ai::stp { @@ -29,6 +30,7 @@ void Play::setWorld(world::World *world) noexcept { this->world = world; } void Play::updateField(world::Field field) noexcept { this->field = field; } void Play::update() noexcept { + ZoneScopedN("Play Update"); // clear roleStatuses so it only contains the current tick's statuses roleStatuses.clear(); // RTT_INFO("Play executing: ", getName()) diff --git a/src/stp/Skill.cpp b/src/stp/Skill.cpp index 190c42a127..ed590e949d 100644 --- a/src/stp/Skill.cpp +++ b/src/stp/Skill.cpp @@ -6,6 +6,7 @@ #include "control/ControlModule.h" #include "world/World.hpp" +#include namespace rtt::ai::stp { @@ -28,6 +29,7 @@ void Skill::refreshRobotCommand() noexcept { void Skill::terminate() noexcept {} Status Skill::update(StpInfo const& info) noexcept { + ZoneScopedN("Skill Update"); robot = info.getRobot(); auto result = onUpdate(info); currentStatus = result; diff --git a/src/stp/Tactic.cpp b/src/stp/Tactic.cpp index e794dde48f..04c6e0f3a8 100644 --- a/src/stp/Tactic.cpp +++ b/src/stp/Tactic.cpp @@ -5,6 +5,7 @@ #include "stp/Tactic.h" #include +#include namespace rtt::ai::stp { @@ -15,6 +16,7 @@ void Tactic::initialize() noexcept { } Status Tactic::update(StpInfo const &info) noexcept { + ZoneScopedN("Tactic Update"); // Update skill info auto skill_info = calculateInfoForSkill(info); diff --git a/src/stp/computations/PassComputations.cpp b/src/stp/computations/PassComputations.cpp index cfb586e32f..f52f778f26 100644 --- a/src/stp/computations/PassComputations.cpp +++ b/src/stp/computations/PassComputations.cpp @@ -10,6 +10,7 @@ #include "control/ControlUtils.h" #include "roboteam_utils/Tube.h" #include "stp/constants/ControlConstants.h" +#include "utilities/GameStateManager.hpp" namespace rtt::ai::stp::computations { diff --git a/src/stp/skills/GoToPos.cpp b/src/stp/skills/GoToPos.cpp index 51afe5dd28..23fa8e94cb 100644 --- a/src/stp/skills/GoToPos.cpp +++ b/src/stp/skills/GoToPos.cpp @@ -4,35 +4,37 @@ #include "stp/skills/GoToPos.h" -#include "control/positionControl/BBTrajectories/WorldObjects.h" +#include "roboteam_utils/Print.h" +#include "roboteam_utils/Timer.h" #include "world/World.hpp" +#include namespace rtt::ai::stp::skill { Status GoToPos::onUpdate(const StpInfo &info) noexcept { + ZoneScopedN("GoToPos"); + auto &robotValue = robot.value(); Vector2 targetPos = info.getPositionToMoveTo().value(); + if (!targetPos.isNotNaN()) { + RTT_ERROR("GoToPos: targetPos is nan for robot id: ", robotValue->getId()); + return Status::Failure; + } + if (!FieldComputations::pointIsValidPosition(info.getField().value(), targetPos, info.getObjectsToAvoid())) { RTT_WARNING("Target point is not a valid position for robot id: ", info.getRobot().value()->getId()) targetPos = FieldComputations::projectPointToValidPosition(info.getField().value(), targetPos, info.getObjectsToAvoid()); } - bool useOldPathPlanning = false; - rtt::BB::CommandCollision commandCollision; - - if (useOldPathPlanning) { - // Calculate commands from path planning and tracking - commandCollision.robotCommand = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackPath( - info.getField().value(), info.getRobot().value()->getId(), info.getRobot().value()->getPos(), info.getRobot().value()->getVel(), targetPos, info.getPidType().value()); - } else { - // _______Use this one for the BBT pathplanning and tracking_______ - commandCollision = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory( - info.getCurrentWorld(), info.getField().value(), info.getRobot().value()->getId(), info.getRobot().value()->getPos(), info.getRobot().value()->getVel(), targetPos, - info.getMaxRobotVelocity(), info.getPidType().value(), info.getObjectsToAvoid()); - } + auto controlCommand = + info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory(info.getField().value(), robotValue->getId(), robotValue->getPos(), robotValue->getVel(), + targetPos, info.getMaxRobotVelocity(), info.getPidType().value(), info.getObjectsToAvoid()); - if (commandCollision.collisionData.has_value()) { - // return Status::Failure; + if (controlCommand.isOccupied) { + // TODO: This should NOT be neccessary + command.velocity = Vector2(0, 0); + forwardRobotCommand(info.getCurrentWorld()); + return Status::Failure; } double targetVelocityLength; @@ -40,10 +42,9 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { RTT_DEBUG("Setting max vel"); targetVelocityLength = info.getMaxRobotVelocity(); } else { - targetVelocityLength = std::clamp(commandCollision.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()); + targetVelocityLength = std::clamp(controlCommand.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()); } - // Clamp and set velocity - Vector2 targetVelocity = commandCollision.robotCommand.velocity.stretchToLength(targetVelocityLength); + Vector2 targetVelocity = controlCommand.robotCommand.velocity.stretchToLength(targetVelocityLength); // Set velocity and angle commands command.velocity = targetVelocity; @@ -58,13 +59,13 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { command.dribblerSpeed = targetDribblerSpeed; // set command ID - command.id = info.getRobot().value()->getId(); + // command.id = robot.value()->getId(); // forward the generated command to the ControlModule, for checking and limiting forwardRobotCommand(info.getCurrentWorld()); // Check if successful - if ((info.getRobot().value()->getPos() - targetPos).length() <= stp::control_constants::GO_TO_POS_ERROR_MARGIN) { + if ((robotValue->getPos() - targetPos).length() <= stp::control_constants::GO_TO_POS_ERROR_MARGIN) { return Status::Success; } else { return Status::Running; diff --git a/src/utilities/Constants.cpp b/src/utilities/Constants.cpp index 0c1f50f06d..7bb4b8729d 100644 --- a/src/utilities/Constants.cpp +++ b/src/utilities/Constants.cpp @@ -252,4 +252,10 @@ std::vector Constants::ruleSets() { {"kickoff", 1.5, 6.5, 0.5, 0.0, true}}; } +double Constants::POSITION_CONTROL_MAX_TARGET_DEVIATION() { return 0.05; } + +double Constants::POSITION_CONTROL_MAX_STILL_VELOCITY() { return 0.05; } + +double Constants::POSITION_CONTROL_MIN_DISTANCE_REACHED() { return ROBOT_RADIUS(); } + } // namespace rtt::ai diff --git a/src/world/World.cpp b/src/world/World.cpp index a5644cb2be..bf6916ff52 100644 --- a/src/world/World.cpp +++ b/src/world/World.cpp @@ -4,6 +4,9 @@ #include "world/World.hpp" +#include "utilities/GameStateManager.hpp" +#include + namespace rtt::world { WorldData const &World::setWorld(WorldData &newWorld) noexcept { if (currentWorld) { @@ -91,10 +94,12 @@ void World::updateTickTime() noexcept { } void World::updatePositionControl() { - std::vector robotPositions(getWorld()->getRobotsNonOwning().size()); - std::transform(getWorld()->getRobotsNonOwning().begin(), getWorld()->getRobotsNonOwning().end(), robotPositions.begin(), - [](const auto &robot) -> Vector2 { return (robot->getPos()); }); - positionControl.setRobotPositions(robotPositions); + ZoneScopedN("Update Position Control"); + auto &collisionDetector = positionControl.getCollisionDetector(); + collisionDetector.setField(getField()); + collisionDetector.updateTimeline(getWorld()->getRobotsNonOwning(), getWorld()->getBall()); + auto gameState = rtt::ai::GameStateManager::getCurrentGameState(); + collisionDetector.setMinBallDistance(gameState.getRuleSet().minDistanceToBall); } uint64_t World::getTimeDifference() const noexcept { return tickDuration; }