Delete and remove all references to Util.h

src/Rover.cpp

@@ -1,6 +1,5 @@
 #include "Constants.h"
 #include "Globals.h"
-#include "Util.h"
 #include "log.h"
 #include "navtypes.h"
 #include "network/MissionControlProtocol.h"

src/commands/DriveToWaypointCommand.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "../navtypes.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 #include "CommandBase.h"
 
@@ -11,44 +12,47 @@ namespace commands {
 
 /**
  * Provides robot drive velocity commands to navigate the robot to a given waypoint given
- * the velocity parameters to drive the robot at, the proportional factor to steer the robot with
- * and the 2D Cartesian position of the target in the world reference frame.
+ * the velocity parameters to drive the robot at, the proportional factor to steer the robot
+ * with and the 2D Cartesian position of the target in the world reference frame.
  */
 class DriveToWaypointCommand : CommandBase {
 public:
 	/**
 	 * Creates a new DriveToWaypointCommand with the specified targeting information.
-	 * 
+	 *
 	 * @param target, the position of the target in world coordinates.
 	 * @param thetaKP, the scaling factor to use for robot steering proportional control.
 	 * @param driveVel, the speed to drive the robot at.
 	 * @param slowDriveVel, the slower speed to drive the robot at when near the target.
 	 * @param doneThresh, the threshold for minimum robot-target distance to complete command.
-	 * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation 
+	 * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation
 	 * 							   is considered done, seconds.
 	 */
 	DriveToWaypointCommand(const navtypes::point_t& target, double thetaKP, double driveVel,
-						   double slowDriveVel, double doneThresh, util::dseconds closeToTargetDur);
+						   double slowDriveVel, double doneThresh,
+						   util::dseconds closeToTargetDur);
 
 	/**
-	 * Must be called before getOutput() for each iteration. Updates the current pose of the robot in 
-	 * the global reference frame used by DriveToWaypointCommand.
-	 * 
+	 * Must be called before getOutput() for each iteration. Updates the current pose of the
+	 * robot in the global reference frame used by DriveToWaypointCommand.
+	 *
 	 * @param pose, the current pose of the robot in the global reference frame.
 	 * @param time, the current time from robot::types::dataclock.
 	 */
 	void setState(const navtypes::pose_t& pose, robot::types::datatime_t time);
 
 	/**
-	 * Gets the angular velocity and forward speed to run the robot at in order to navigate robot to target.
-	 * 
-	 * @return the angular velocity and forward speed to run the robot at in order to navigate robot to target.
+	 * Gets the angular velocity and forward speed to run the robot at in order to navigate
+	 * robot to target.
+	 *
+	 * @return the angular velocity and forward speed to run the robot at in order to navigate
+	 * robot to target.
 	 */
 	command_t getOutput() override;
 
 	/**
 	 * Returns whether the DriveToWaypointCommand has finished its navigation task.
-	 * 
+	 *
 	 * @return whether the DriveToWaypointCommand has finished its navigation task.
 	 */
 	bool isDone() override;

src/control/JacobianVelController.h

@@ -1,9 +1,9 @@
 #pragma once
 
-#include "../Util.h"
 #include "../filters/StateSpaceUtil.h"
 #include "../navtypes.h"
 #include "../utils/math.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <functional>
@@ -19,7 +19,8 @@
  * @tparam inputDim The dimension of the input vector of the kinematic function.
  * @see https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant
  */
-template <int outputDim, int inputDim> class JacobianVelController {
+template <int outputDim, int inputDim>
+class JacobianVelController {
 public:
 	/**
 	 * @brief Construct a new controller.

src/control/PIDControl.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../Util.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <chrono>
@@ -27,9 +27,11 @@ struct PIDGains {
  * @tparam dim The number of dimensions to control.
  * @see https://en.wikipedia.org/wiki/PID_controller
  */
-template <int dim> class PIDController {
+template <int dim>
+class PIDController {
 public:
-	template <int d> using Arrayd = Eigen::Array<double, d, 1>;
+	template <int d>
+	using Arrayd = Eigen::Array<double, d, 1>;
 
 	/**
 	 * @brief Construct a new PIDController.

src/control/PlanarArmController.h

@@ -2,6 +2,7 @@
 
 #include "../kinematics/PlanarArmKinematics.h"
 #include "../navtypes.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <array>
@@ -21,7 +22,8 @@ namespace control {
  *
  * @tparam N The number of arm joints.
  */
-template <unsigned int N> class PlanarArmController {
+template <unsigned int N>
+class PlanarArmController {
 public:
 	/**
 	 * @brief Construct a new controller object.

src/control/TrapezoidalVelocityProfile.h

@@ -1,7 +1,7 @@
 #pragma once
 
-#include "../Util.h"
 #include "../navtypes.h"
+#include "../utils/time.h"
 #include "../world_interface/data.h"
 
 #include <array>
@@ -19,7 +19,8 @@ namespace control {
  *
  * @tparam dim The number of dimensions that are controlled by this profile.
  */
-template <int dim> class TrapezoidalVelocityProfile {
+template <int dim>
+class TrapezoidalVelocityProfile {
 public:
 	/**
 	 * @brief Construct a new velocity profile object.

src/filters/FullPoseEstimator.cpp

@@ -1,7 +1,7 @@
 #include "FullPoseEstimator.h"
 
-#include "../Util.h"
 #include "../navtypes.h"
+#include "../utils/transform.h"
 
 #include <functional>
 using namespace kinematics;

src/filters/PoseEstimator.cpp

@@ -1,7 +1,7 @@
 #include "PoseEstimator.h"
 
-#include "../Util.h"
 #include "../navtypes.h"
+#include "../utils/transform.h"
 
 using namespace navtypes;
 using namespace kinematics;

src/filters/PoseEstimatorLinear.cpp

@@ -1,6 +1,6 @@
 #include "PoseEstimatorLinear.h"
 
-#include "../Util.h"
+#include "../utils/transform.h"
 
 using util::toPose;
 using util::toTransform;

src/filters/pose_graph/friendly_graph.cpp

@@ -3,7 +3,7 @@
 
 #include "../../Constants.h"
 #include "../../navtypes.h"
-#include "../../Util.h"
+#include "../../utils/transform.h"
 
 #include <iostream>
 #include <vector>

src/kinematics/DiffDriveKinematics.cpp

@@ -1,6 +1,6 @@
 #include "DiffDriveKinematics.h"
 
-#include "../Util.h"
+#include "../utils/transform.h"
 using namespace navtypes;
 using util::toTransformRotateFirst;
 

src/kinematics/SwerveDriveKinematics.cpp

@@ -1,6 +1,6 @@
 #include "SwerveDriveKinematics.h"
 
-#include "../Util.h"
+#include "../utils/transform.h"
 
 #include <Eigen/QR>
 namespace kinematics {

src/planning/plan.cpp

@@ -2,8 +2,9 @@
 #include "plan.h"
 
 #include "../Constants.h"
-#include "../Util.h"
 #include "../log.h"
+#include "../utils/ScopedTimer.h"
+#include "../utils/transform.h"
 
 #include <iostream>
 #include <queue>

src/world_interface/data.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include "../Util.h"
 #include "../navtypes.h"
 
 #include <bitset>

src/world_interface/kinematic_common_interface.cpp

@@ -1,9 +1,9 @@
 #include "../Constants.h"
 #include "../Globals.h"
-#include "../Util.h"
 #include "../kinematics/DiffDriveKinematics.h"
 #include "../log.h"
 #include "../navtypes.h"
+#include "../utils/transform.h"
 #include "world_interface.h"
 
 #include <atomic>

src/world_interface/real_world_interface.cpp

@@ -3,12 +3,12 @@
 #include "../CAN/CANUtils.h"
 #include "../Constants.h"
 #include "../Globals.h"
-#include "../Util.h"
 #include "../ardupilot/ArduPilotInterface.h"
 #include "../camera/Camera.h"
 #include "../gps/usb_gps/read_usb_gps.h"
 #include "../log.h"
 #include "../navtypes.h"
+#include "../utils/core.h"
 #include "motor/can_motor.h"
 #include "real_world_constants.h"
 #include "world_interface.h"

src/world_interface/simulator_interface.cpp

@@ -1,6 +1,5 @@
 #include "../Constants.h"
 #include "../Globals.h"
-#include "../Util.h"
 #include "../ar/read_landmarks.h"
 #include "../base64/base64_img.h"
 #include "../camera/Camera.h"
@@ -9,6 +8,8 @@
 #include "../log.h"
 #include "../navtypes.h"
 #include "../network/websocket/WebSocketProtocol.h"
+#include "../utils/core.h"
+#include "../utils/transform.h"
 #include "motor/sim_motor.h"
 #include "world_interface.h"
 
@@ -287,7 +288,7 @@ void emergencyStop() {
 	}
 }
 
-std::unordered_set<CameraID> getCameras(){
+std::unordered_set<CameraID> getCameras() {
 	std::shared_lock<std::shared_mutex> lock(cameraFrameMapMutex);
 	return util::keySet(cameraLastFrameIdxMap);
 }

tests/util/UtilTest.cpp

@@ -1,8 +1,8 @@
-#include <catch2/catch.hpp>
+#include "../../src/utils/time.h"
 
 #include <chrono>
 
-#include "../../src/Util.h"
+#include <catch2/catch.hpp>
 
 using namespace Catch::literals;
 using namespace util;