From 6c182970344da5fe44a3b9a612425291a1e7a3b6 Mon Sep 17 00:00:00 2001 From: Quinn Pfeifer <72775766+QuinnP2910@users.noreply.github.com> Date: Sat, 28 Oct 2023 11:47:02 -0700 Subject: [PATCH] Delete and remove all references to Util.h (#277) * Delete and remove all references to Util.h * Resolve additional util imports * Fix formatting --- src/Rover.cpp | 1 - src/Util.h | 7 --- src/commands/DriveToWaypointCommand.h | 28 +++++---- src/control/JacobianVelController.h | 5 +- src/control/PIDControl.h | 8 ++- src/control/PlanarArmController.h | 4 +- src/control/TrapezoidalVelocityProfile.h | 5 +- src/filters/FullPoseEstimator.cpp | 2 +- src/filters/PoseEstimator.cpp | 2 +- src/filters/PoseEstimatorLinear.cpp | 2 +- src/filters/pose_graph/friendly_graph.cpp | 2 +- src/kinematics/DiffDriveKinematics.cpp | 2 +- src/kinematics/SwerveDriveKinematics.cpp | 2 +- src/planning/plan.cpp | 3 +- src/world_interface/data.h | 1 - .../kinematic_common_interface.cpp | 2 +- src/world_interface/real_world_interface.cpp | 2 +- src/world_interface/simulator_interface.cpp | 5 +- tests/util/UtilTest.cpp | 4 +- tests/worldmap/GlobalMapTest.cpp | 59 +++++++------------ tests/worldmap/TrICPTest.cpp | 20 +++---- 21 files changed, 75 insertions(+), 91 deletions(-) delete mode 100644 src/Util.h diff --git a/src/Rover.cpp b/src/Rover.cpp index 0033d1009..25a0a862c 100644 --- a/src/Rover.cpp +++ b/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" diff --git a/src/Util.h b/src/Util.h deleted file mode 100644 index 2113a664f..000000000 --- a/src/Util.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -#include "utils/ScopedTimer.h" -#include "utils/core.h" -#include "utils/random.h" -#include "utils/time.h" -#include "utils/transform.h" diff --git a/src/commands/DriveToWaypointCommand.h b/src/commands/DriveToWaypointCommand.h index 2901f78b3..e6c50ac2d 100644 --- a/src/commands/DriveToWaypointCommand.h +++ b/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; diff --git a/src/control/JacobianVelController.h b/src/control/JacobianVelController.h index 0001e615c..e06323e79 100644 --- a/src/control/JacobianVelController.h +++ b/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 @@ -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 class JacobianVelController { +template +class JacobianVelController { public: /** * @brief Construct a new controller. diff --git a/src/control/PIDControl.h b/src/control/PIDControl.h index b07a2984a..ac888c065 100644 --- a/src/control/PIDControl.h +++ b/src/control/PIDControl.h @@ -1,6 +1,6 @@ #pragma once -#include "../Util.h" +#include "../utils/time.h" #include "../world_interface/data.h" #include @@ -27,9 +27,11 @@ struct PIDGains { * @tparam dim The number of dimensions to control. * @see https://en.wikipedia.org/wiki/PID_controller */ -template class PIDController { +template +class PIDController { public: - template using Arrayd = Eigen::Array; + template + using Arrayd = Eigen::Array; /** * @brief Construct a new PIDController. diff --git a/src/control/PlanarArmController.h b/src/control/PlanarArmController.h index 01c4719f7..a4a1097d0 100644 --- a/src/control/PlanarArmController.h +++ b/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 @@ -21,7 +22,8 @@ namespace control { * * @tparam N The number of arm joints. */ -template class PlanarArmController { +template +class PlanarArmController { public: /** * @brief Construct a new controller object. diff --git a/src/control/TrapezoidalVelocityProfile.h b/src/control/TrapezoidalVelocityProfile.h index 7c9874f28..846ff1bc7 100644 --- a/src/control/TrapezoidalVelocityProfile.h +++ b/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 @@ -19,7 +19,8 @@ namespace control { * * @tparam dim The number of dimensions that are controlled by this profile. */ -template class TrapezoidalVelocityProfile { +template +class TrapezoidalVelocityProfile { public: /** * @brief Construct a new velocity profile object. diff --git a/src/filters/FullPoseEstimator.cpp b/src/filters/FullPoseEstimator.cpp index 17e6a2504..82e34ecf8 100644 --- a/src/filters/FullPoseEstimator.cpp +++ b/src/filters/FullPoseEstimator.cpp @@ -1,7 +1,7 @@ #include "FullPoseEstimator.h" -#include "../Util.h" #include "../navtypes.h" +#include "../utils/transform.h" #include using namespace kinematics; diff --git a/src/filters/PoseEstimator.cpp b/src/filters/PoseEstimator.cpp index 0aa33b7d0..61b42c049 100644 --- a/src/filters/PoseEstimator.cpp +++ b/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; diff --git a/src/filters/PoseEstimatorLinear.cpp b/src/filters/PoseEstimatorLinear.cpp index 45310c363..225b1b88d 100644 --- a/src/filters/PoseEstimatorLinear.cpp +++ b/src/filters/PoseEstimatorLinear.cpp @@ -1,6 +1,6 @@ #include "PoseEstimatorLinear.h" -#include "../Util.h" +#include "../utils/transform.h" using util::toPose; using util::toTransform; diff --git a/src/filters/pose_graph/friendly_graph.cpp b/src/filters/pose_graph/friendly_graph.cpp index d81c94acf..73c034e86 100644 --- a/src/filters/pose_graph/friendly_graph.cpp +++ b/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 #include diff --git a/src/kinematics/DiffDriveKinematics.cpp b/src/kinematics/DiffDriveKinematics.cpp index 27ca5e158..afdd84855 100644 --- a/src/kinematics/DiffDriveKinematics.cpp +++ b/src/kinematics/DiffDriveKinematics.cpp @@ -1,6 +1,6 @@ #include "DiffDriveKinematics.h" -#include "../Util.h" +#include "../utils/transform.h" using namespace navtypes; using util::toTransformRotateFirst; diff --git a/src/kinematics/SwerveDriveKinematics.cpp b/src/kinematics/SwerveDriveKinematics.cpp index 4f04dc4ff..181f59983 100644 --- a/src/kinematics/SwerveDriveKinematics.cpp +++ b/src/kinematics/SwerveDriveKinematics.cpp @@ -1,6 +1,6 @@ #include "SwerveDriveKinematics.h" -#include "../Util.h" +#include "../utils/transform.h" #include namespace kinematics { diff --git a/src/planning/plan.cpp b/src/planning/plan.cpp index d0eb34721..68cebecc5 100644 --- a/src/planning/plan.cpp +++ b/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 #include diff --git a/src/world_interface/data.h b/src/world_interface/data.h index 550999074..9ef92d604 100644 --- a/src/world_interface/data.h +++ b/src/world_interface/data.h @@ -1,6 +1,5 @@ #pragma once -#include "../Util.h" #include "../navtypes.h" #include diff --git a/src/world_interface/kinematic_common_interface.cpp b/src/world_interface/kinematic_common_interface.cpp index e72e383be..5c0c35ccf 100644 --- a/src/world_interface/kinematic_common_interface.cpp +++ b/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 diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index a04836fca..7e1bda0f6 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/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" diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index e51b78e01..ad755d077 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/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 getCameras(){ +std::unordered_set getCameras() { std::shared_lock lock(cameraFrameMapMutex); return util::keySet(cameraLastFrameIdxMap); } diff --git a/tests/util/UtilTest.cpp b/tests/util/UtilTest.cpp index f0276d71b..bb8468c86 100644 --- a/tests/util/UtilTest.cpp +++ b/tests/util/UtilTest.cpp @@ -1,8 +1,8 @@ -#include +#include "../../src/utils/time.h" #include -#include "../../src/Util.h" +#include using namespace Catch::literals; using namespace util; diff --git a/tests/worldmap/GlobalMapTest.cpp b/tests/worldmap/GlobalMapTest.cpp index 1294fe7c5..5c4296367 100644 --- a/tests/worldmap/GlobalMapTest.cpp +++ b/tests/worldmap/GlobalMapTest.cpp @@ -1,25 +1,23 @@ #include "../../src/worldmap/GlobalMap.h" + #include "../../src/navtypes.h" -#include "../../src/Util.h" +#include "../../src/utils/transform.h" -#include -#include #include +#include +#include #include using namespace navtypes; using namespace util; -namespace -{ -double rand(double low, double high) -{ +namespace { +double rand(double low, double high) { return low + (::rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp) } -transform_t randTransform(double centerX, double centerY) -{ +transform_t randTransform(double centerX, double centerY) { // move back to the origin, rotate around the origin, then move back. transform_t toOrigin = toTransformRotateFirst(centerX, centerY, 0); transform_t fromOrigin = toTransformRotateFirst(-centerX, -centerY, 0); @@ -28,12 +26,10 @@ transform_t randTransform(double centerX, double centerY) return fromOrigin * trf * toOrigin; } -double calculateMSE(const points_t &p1, const points_t &p2) -{ +double calculateMSE(const points_t& p1, const points_t& p2) { double mse = 0; REQUIRE(p2.size() >= p1.size()); - for (size_t i = 0; i < p1.size(); i++) - { + for (size_t i = 0; i < p1.size(); i++) { point_t truth = p1[i]; point_t p = p2[i]; mse += pow((p - truth).norm(), 2); @@ -44,8 +40,7 @@ double calculateMSE(const points_t &p1, const points_t &p2) } } // namespace -TEST_CASE("Global Map - ScanStride", "[GlobalMap]") -{ +TEST_CASE("Global Map - ScanStride", "[GlobalMap]") { srand(time(nullptr)); // NOLINT(cert-msc51-cpp) for (int i = 1; i <= 3; i++) { @@ -62,8 +57,7 @@ TEST_CASE("Global Map - ScanStride", "[GlobalMap]") } } -TEST_CASE("Global Map - GetClosest", "[GlobalMap]") -{ +TEST_CASE("Global Map - GetClosest", "[GlobalMap]") { srand(time(nullptr)); // NOLINT(cert-msc51-cpp) GlobalMap map(1000); @@ -72,8 +66,7 @@ TEST_CASE("Global Map - GetClosest", "[GlobalMap]") constexpr int areaMin = -5; constexpr int areaMax = 5; - for (int i = 0; i < 100; i++) - { + for (int i = 0; i < 100; i++) { double x = rand(areaMin, areaMax); double y = rand(areaMin, areaMax); point_t p = {x, y, 1}; @@ -89,11 +82,9 @@ TEST_CASE("Global Map - GetClosest", "[GlobalMap]") // manually compute the nearest neighbor to check against point_t closest = {0, 0, 0}; double minDist = std::numeric_limits::infinity(); - for (const point_t &p : points) - { + for (const point_t& p : points) { double dist = (p - point).topRows<2>().norm(); - if (dist < minDist) - { + if (dist < minDist) { minDist = dist; closest = p; } @@ -111,20 +102,17 @@ TEST_CASE("Global Map - GetClosest", "[GlobalMap]") } } -TEST_CASE("Global Map", "[GlobalMap]") -{ +TEST_CASE("Global Map", "[GlobalMap]") { srand(time(nullptr)); // NOLINT(cert-msc51-cpp) GlobalMap map(1000); points_t allPoints; // create 5 groups of points for aligning with each other, all in same area - for (int i = 0; i < 5; i++) - { + for (int i = 0; i < 5; i++) { points_t sample; // first sample should be in axes reference frame, so we can check performance easily transform_t trf = i > 0 ? randTransform(0, 0) : transform_t::Identity(); - for (int j = 0; j < 150; j++) - { + for (int j = 0; j < 150; j++) { double x1 = rand(-3, 3); double y1 = pow(x1, 3); point_t p = {x1, y1, 1}; @@ -137,7 +125,7 @@ TEST_CASE("Global Map", "[GlobalMap]") points_t mapPoints = map.getPoints(); // we need to sort because iteration order is not consistent - auto comp = [](const point_t &a, const point_t &b) { return a(0) < b(0); }; + auto comp = [](const point_t& a, const point_t& b) { return a(0) < b(0); }; std::sort(allPoints.begin(), allPoints.end(), comp); std::sort(mapPoints.begin(), mapPoints.end(), comp); @@ -147,8 +135,7 @@ TEST_CASE("Global Map", "[GlobalMap]") REQUIRE(mse == Approx(0).margin(0.5)); } -TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") -{ +TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") { srand(time(nullptr)); // NOLINT(cert-msc51-cpp) // we'll be getting points along the sin function std::function func = [](double x) { return sin(x); }; @@ -156,8 +143,7 @@ TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") GlobalMap map(1000); points_t truths; // create 5 groups of points for aligning, each partially overlapping with the last - for (int i = -2; i <= 2; i++) - { + for (int i = -2; i <= 2; i++) { // the area is 3 wide, and the center moves right by 1.5 each time double center = i * 1.5; double min = center - 1.5; @@ -166,8 +152,7 @@ TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") // last sample should be in axes reference frame, so we can check performance easily transform_t trf = i != 2 ? randTransform(center, func(center)) : transform_t::Identity(); - for (int j = 0; j < 100; j++) - { + for (int j = 0; j < 100; j++) { double x = rand(min, max); double y = func(x); point_t p = {x, y, 1}; @@ -181,7 +166,7 @@ TEST_CASE("Global Map 0.5 Overlap", "[GlobalMap]") points_t mapPoints = map.getPoints(); // we need to sort because iteration order is not consistent - auto comp = [](const point_t &a, const point_t &b) { return a(0) < b(0); }; + auto comp = [](const point_t& a, const point_t& b) { return a(0) < b(0); }; std::sort(truths.begin(), truths.end(), comp); std::sort(mapPoints.begin(), mapPoints.end(), comp); diff --git a/tests/worldmap/TrICPTest.cpp b/tests/worldmap/TrICPTest.cpp index 2d6f58b99..14722b1e7 100644 --- a/tests/worldmap/TrICPTest.cpp +++ b/tests/worldmap/TrICPTest.cpp @@ -1,33 +1,29 @@ #include "../../src/worldmap/TrICP.h" -#include "../../src/Util.h" -#include +#include "../../src/utils/transform.h" +#include "../../src/worldmap/GlobalMap.h" + #include +#include #include -#include "../../src/worldmap/GlobalMap.h" - using namespace navtypes; using namespace util; -namespace -{ -double rand(double low, double high) -{ +namespace { +double rand(double low, double high) { return low + (::rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp) } } // namespace -TEST_CASE("Trimmed ICP") -{ +TEST_CASE("Trimmed ICP") { points_t map; points_t sample; points_t truths; transform_t trf = toTransformRotateFirst(0.1, -0.25, M_PI / 24); srand(time(nullptr)); // NOLINT(cert-msc51-cpp) - for (int i = 0; i < 150; i++) - { + for (int i = 0; i < 150; i++) { double x1 = rand(-6, 2); double y1 = pow(x1, 3); map.push_back({x1, y1, 1});