Merge pull request #113 from RoboTeamTwente/Update/path-planning

Change path planning scoring

roboteam_ai/include/roboteam_ai/control/positionControl/BBTrajectories/WorldObjects.h

@@ -14,18 +14,21 @@
 
 namespace rtt::BB {
 
+/**
+ * @brief Enum that stores the different types of collisions
+ */
+enum class CollisionType { FIELD, DEFENSEAREA, BALL, ENEMYROBOT, OWNROBOT, BALLPLACEMENT };
+
 /**
  * @brief Struct that stores data about a collision
- * @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
+ * @memberof collisionType type of collision
  */
 struct CollisionData {
-    Vector2 obstaclePosition;
     Vector2 collisionPosition;
     double collisionTime;
-    std::string collisionName;
+    CollisionType collisionType;
 };
 
 /**
@@ -34,8 +37,8 @@ struct CollisionData {
  * @memberof collisionData Data about the collision
  */
 struct CommandCollision {
-    RobotCommand robotCommand;
-    std::optional<CollisionData> collisionData;
+    RobotCommand robotCommand = {};
+    std::optional<CollisionData> collisionData = std::nullopt;
 };
 
 class WorldObjects {
@@ -59,10 +62,25 @@ class WorldObjects {
      * @param computedPaths the paths of our robots
      * @param robotId Id of the robot
      * @param avoidObjects a struct with if it should avoid certain objects
+     * @param completedTimeSteps Number of completed time steps of the trajectory
+     * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory
      * @return optional with rtt::BB::CollisionData
      */
     std::optional<CollisionData> getFirstCollision(const rtt::world::World *world, const rtt::Field &field, const rtt::Trajectory2D &Trajectory,
-                                                   const std::unordered_map<int, std::vector<Vector2>> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects);
+                                                   const std::unordered_map<int, std::vector<Vector2>> &computedPaths, int robotId, ai::stp::AvoidObjects avoidObjects,
+                                                   std::unordered_map<int, int> completedTimeSteps, double startTime = 0);
+
+    /**
+     * @brief Calculates the position of the first collision with the defense area, note that this functions assumes the entire trajectory,
+     * including the part that might already be completed. Only call this function for a new trajectory
+     * @param field used for checking collisions with the field
+     * @param BBTrajectory the trajectory to check for collisions
+     * @param computedPaths the paths of our robots
+     * @param robotId Id of the robot
+     * @return optional with rtt::BB::CollisionData
+     */
+    std::optional<CollisionData> getFirstDefenseAreaCollision(const rtt::Field &field, const rtt::Trajectory2D &Trajectory,
+                                                              const std::unordered_map<int, std::vector<Vector2>> &computedPaths, int robotId);
 
     /**
      * @brief Takes a calculated path of a robot and checks points along the path if they are outside the
@@ -99,9 +117,10 @@ class WorldObjects {
      * @param timeStep Time between pathpoints
      * @param dist Distance to the ball
      * @param completedTimeSteps Number of completed time steps
+     * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory
      */
-    void calculateBallCollisions(const rtt::world::World *world, std::vector<CollisionData> &collisionDatas, std::vector<Vector2> pathPoints, double timeStep, double dist,
-                                 size_t completedTimeSteps);
+    void calculateBallCollisions(const rtt::world::World *world, std::vector<CollisionData> &collisionDatas, const std::vector<Vector2> &pathPoints, double timeStep, double dist,
+                                 size_t completedTimeSteps, double startTime);
 
     /**
      * @brief Takes a calculated path of a robot and checks points along the path if they are too close to an
@@ -112,9 +131,11 @@ class WorldObjects {
      * @param pathPoints std::vector with path points
      * @param timeStep Time between pathpoints
      * @param completedTimeSteps Amount of seconds of the trajectory already completed by the robot
+     * @param avoidId ID of the robot to ignore collisions with
+     * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory
      */
     void calculateEnemyRobotCollisions(const rtt::world::World *world, std::vector<CollisionData> &collisionDatas, const std::vector<Vector2> &pathPoints, double timeStep,
-                                       size_t completedTimeSteps);
+                                       size_t completedTimeSteps, int avoidId, double startTime);
 
     /**
      * @brief Takes a path from the array of stored paths and checks points along the path if they are too close to
@@ -127,9 +148,24 @@ class WorldObjects {
      * @param robotId ID of the robot
      * @param timeStep Time between pathpoints
      * @param completedTimeSteps how many seconds of the trajectory is already completed by the robot
+     * @param startTime Start time of the current trajectory part that is being checked. This is the time that it takes for the robot to reach the first point of the trajectory
      */
     void calculateOurRobotCollisions(const rtt::world::World *world, std::vector<CollisionData> &collisionDatas, const std::vector<Vector2> &pathPoints,
-                                     const std::unordered_map<int, std::vector<Vector2>> &computedPaths, int robotId, double timeStep, size_t completedTimeSteps);
+                                     const std::unordered_map<int, std::vector<Vector2>> &computedPaths, int robotId, double timeStep,
+                                     std::unordered_map<int, int> completedTimeSteps, double startTime);
+
+    /**
+     * @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 Time between pathpoints
+     * @param completedTimeSteps Number of completed time steps
+     */
+    void calculateBallPlacementCollision(const rtt::world::World *world, std::vector<CollisionData> &collisionDatas, const std::vector<Vector2> &pathPoints, double timeStep,
+                                         size_t completedTimeSteps);
 
     /**
      * @brief Inserts collisionData in the vector collisionDatas such that they are ordered from lowest collisionTime to highest

roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h

@@ -34,6 +34,7 @@ class PositionControl {
     std::unordered_map<int, std::vector<Vector2>> computedPaths;                           /**< Map of computed paths for each robot */
     std::unordered_map<int, std::vector<Vector2>> computedPathsVel;                        /**< Map of computed velocities for each robot */
     std::unordered_map<int, std::vector<std::pair<Vector2, Vector2>>> computedPathsPosVel; /**< Map of pairs containing position and velocity for each robot */
+    std::unordered_map<int, int> completedTimeSteps;                                       /**< Map of completed time steps for each robot */
 
     /**
      * @brief Checks if the current path should be recalculated:
@@ -104,10 +105,25 @@ class PositionControl {
     rtt::BB::CommandCollision computeAndTrackTrajectory(const rtt::world::World *world, const rtt::Field &field, int robotId, Vector2 currentPosition, Vector2 currentVelocity,
                                                         Vector2 targetPosition, double maxRobotVelocity, stp::PIDType pidType, stp::AvoidObjects avoidObjects);
 
+    /**
+     * @brief Calculates a score for a trajectory. The score is based on the distance to the target and the
+     * distance to the first collision on the path
+     * @param world a pointer to the current world
+     * @param field the field object, used onwards by the collision detector
+     * @param robotId the ID of the robot for which the path is calculated
+     * @param firstCollision location of the first collision on the current path
+     * @param trajectoryAroundCollision the trajectory to the intermediate point
+     * @param avoidObjects whether or not to avoid objects
+     * @param startTime the time at which the trajectory starts
+     * @return A score for the trajectory
+     */
+    double calculateScore(const rtt::world::World *world, const rtt::Field &field, int robotId, std::optional<BB::CollisionData> &firstCollision,
+                          Trajectory2D &trajectoryAroundCollision, stp::AvoidObjects avoidObjects, double startTime = 0);
+
     /**
      * @brief Tries to find a new trajectory when the current path has a collision on it. It tries this by
      * 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
+     * paths again to the target
      * @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
@@ -124,41 +140,14 @@ class PositionControl {
                                                   stp::AvoidObjects AvoidObjects);
 
     /**
-     * @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<Trajectory2D> calculateTrajectoryAroundCollision(const rtt::world::World *world, const rtt::Field &field,
-                                                                   std::optional<BB::CollisionData> &intermediatePathCollision, Trajectory2D trajectoryToIntermediatePoint,
-                                                                   Vector2 &targetPosition, int robotId, double maxRobotVelocity, double timeStep, stp::AvoidObjects avoidObjects);
-
-    /**
-     * @brief 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
+     * @brief Creates intermediate points to make a path to. These points all have equal distance to the
+     * collision point
      * @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
      */
-    std::vector<Vector2> createIntermediatePoints(const rtt::Field &field, int robotId, std::optional<BB::CollisionData> &firstCollision, Vector2 &targetPosition);
-
-    /**
-     * @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
-     */
-    std::priority_queue<std::pair<double, Vector2>, std::vector<std::pair<double, Vector2>>, std::greater<>> scoreIntermediatePoints(
-        std::vector<Vector2> &intermediatePoints, std::optional<BB::CollisionData> &firstCollision);
+    std::vector<Vector2> createIntermediatePoints(const rtt::Field &field, std::optional<BB::CollisionData> &firstCollision, Vector2 &targetPosition);
 };
 
 }  // namespace rtt::ai::control

roboteam_ai/include/roboteam_ai/stp/StpInfo.h

@@ -47,9 +47,6 @@ struct StpInfo {
     const std::optional<Vector2>& getPositionToDefend() const { return positionToDefend; }
     void setPositionToDefend(const std::optional<Vector2>& position) { this->positionToDefend = position; }
 
-    const Vector2& getTargetLocationSpeed() const { return targetLocationSpeed; }
-    void setTargetLocationSpeed(const Vector2& speed) { this->targetLocationSpeed = speed; }
-
     double getKickChipVelocity() const { return kickChipVelocity; }
     void setKickChipVelocity(double kickChipVelocity) { this->kickChipVelocity = kickChipVelocity; }
 
@@ -94,6 +91,8 @@ struct StpInfo {
 
     void setShouldAvoidTheirRobots(bool shouldAvoidTheirRobots) { avoidObjects.shouldAvoidTheirRobots = shouldAvoidTheirRobots; }
 
+    void setNotAvoidTheirRobotId(int notAvoidTheirRobotId) { avoidObjects.notAvoidTheirRobotId = notAvoidTheirRobotId; }
+
    private:
     /**
      * Current world pointer
@@ -135,11 +134,6 @@ struct StpInfo {
      */
     std::optional<Vector2> positionToDefend;
 
-    /**
-     * Speed of the target location (e.g. speed of the robot or speed of the ball)
-     */
-    Vector2 targetLocationSpeed = Vector2(0, 0);
-
     /**
      * Velocity of the kick/chip
      */

roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h

@@ -69,12 +69,15 @@ constexpr double ROBOT_CLOSE_TO_POINT = 0.2;                  /**< Distance from
 constexpr double DISTANCE_TO_ROBOT_NEAR = 2.2 * ROBOT_RADIUS; /**< Distance from the robot to another robot at which the robot is considered near that other robot */
 constexpr double DEFENSE_AREA_AVOIDANCE_MARGIN = 0.1;         /**< Distance error for avoiding the defense area */
 constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */
+constexpr double BALL_PLACEMENT_ALMOST_DONE_DISTANCE =
+    0.5; /**< Distance between ball and placement location at which we no longer include the distance to the edges of the placement location in scoring the trajectory */
+constexpr double OUT_OF_FIELD_MARGIN = 0.17; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */
 
 /// Keeper constants
 constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from the keeper to the goal at which the keeper is considered close to that goal */
 
 /// GameState constants
-constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS; /**< Minimum distance all robots should keep when avoiding the ball */
+constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */
 
 }  // namespace rtt::ai::stp::control_constants
 

roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h

@@ -38,11 +38,12 @@ enum class Status { Waiting, Success, Failure, Running };
  */
 struct AvoidObjects {
     bool shouldAvoidBall = false;                                  /***< Indicates whether the robot should avoid the ball */
+    double avoidBallDist = control_constants::AVOID_BALL_DISTANCE; /***< The minimum distance a robot should keep to the ball when avoiding the ball */
     bool shouldAvoidDefenseArea = true;                            /***< Indicates whether the robot should avoid the defense area */
     bool shouldAvoidOutOfField = true;                             /***< Indicates whether the robot should avoid going out of the field */
     bool shouldAvoidOurRobots = true;                              /***< Indicates whether the robot should avoid allied robots */
     bool shouldAvoidTheirRobots = true;                            /***< Indicates whether the robot should avoid the enemy robots */
-    double avoidBallDist = control_constants::AVOID_BALL_DISTANCE; /***< The minimum distance a robot should keep to the ball when avoiding the ball */
+    int notAvoidTheirRobotId = -1;                                 /***< Indicates which robot of the enemy team should not be avoided, -1 means all robots should be avoided */
 };
 }  // namespace rtt::ai::stp
 #endif  // RTT_STPINFOENUMS_H