Refactore/bbt position control

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

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

@@ -10,9 +10,15 @@
 #include <vector>
 
 #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<Vector2> 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<Vector2> getPathApproach(double timeStep) const;
+    [[nodiscard]] std::vector<Vector2> &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<Vector2> getVelocityVector(double timeStep) const;
+    [[nodiscard]] std::vector<Vector2> &getVelocityVector();
 
     /**
      * @brief Transforms the BBTrajectory into a posVelVector at specified timeSteps
      */
-    [[nodiscard]] std::vector<std::pair<Vector2, Vector2>> getPosVelVector(double timeStep);
+    [[nodiscard]] std::vector<PosVelVector> 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<Vector2> positions;
+    std::vector<Vector2> velocities;
 };
 }  // namespace rtt::BB
 #endif  // RTT_BBTRAJECTORY2D_H