From 14b8afe972d918ea338b262f2467a32e23caff1b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Nov 2023 19:29:55 +0100 Subject: [PATCH] PositionSmoothingTest: remove duplicate vector comparison --- src/lib/motion_planning/PositionSmoothing.cpp | 2 -- .../motion_planning/PositionSmoothingTest.cpp | 32 ++++--------------- 2 files changed, 6 insertions(+), 28 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 8f05f39e2d..28c70c690d 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -35,8 +35,6 @@ #include "TrajectoryConstraints.hpp" #include #include -#include - void PositionSmoothing::_generateSetpoints( const Vector3f &position, diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp index 9ce32d9fd5..12faa7a102 100644 --- a/src/lib/motion_planning/PositionSmoothingTest.cpp +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -55,23 +55,6 @@ public: _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.f}, {0.f, 0.f, 0.f}); } - static bool _vectorNear(const Vector3f &a, const Vector3f &b, float EPS = 1e-4f) - { - return (fabsf(a(0) - b(0)) < EPS) && (fabsf(a(1) - b(1)) < EPS) && (fabsf(a(2) - b(2)) < EPS); - } - - static void expectVectorEqual(const Vector3f &expected, const Vector3f &value, const char *name, float EPS) - { - EXPECT_TRUE(_vectorNear(expected, value, EPS)) << - "Vector \"" << name << "\" expected [" << - expected(0) << ", " << - expected(1) << ", " << - expected(2) << "] has value [" << - value(0) << ", " << - value(1) << ", " << - value(2) << "]\n"; - } - static void expectDynamicsLimitsRespected(const PositionSmoothing::PositionSmoothingSetpoints &setpoints) { EXPECT_LE(fabsf(setpoints.velocity(0)), MAX_VELOCITY) << "Velocity in x too high\n"; @@ -89,7 +72,6 @@ public: TEST_F(PositionSmoothingTest, reachesTargetPositionSetpoint) { - const float EPS = 1e-4; const int N_ITER = 2000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; @@ -116,20 +98,19 @@ TEST_F(PositionSmoothingTest, reachesTargetPositionSetpoint) position = out.position; expectDynamicsLimitsRespected(out); - if ((position - TARGET).length() < EPS) { + if (position == TARGET) { printf("Converged in %d iterations\n", iteration); break; } } - expectVectorEqual(TARGET, position, "position", EPS); + EXPECT_EQ(TARGET, position); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; } TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration) { - const float EPS = 1e-4; const int N_ITER = 2000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; @@ -157,20 +138,19 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration) expectDynamicsLimitsRespected(out); - if ((position - TARGET).length() < EPS) { + if (position == TARGET) { printf("Converged in %d iterations\n", iteration); break; } } - expectVectorEqual(TARGET, position, "position", EPS); + EXPECT_EQ(TARGET, position); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; } TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) { - const float EPS = 1e-4; const int N_ITER = 2000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; @@ -200,12 +180,12 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) ff_velocity = {0.f, 0.f, 0.f}; expectDynamicsLimitsRespected(out); - if ((position - TARGET).length() < EPS) { + if (position == TARGET) { printf("Converged in %d iterations\n", iteration); break; } } - expectVectorEqual(TARGET, position, "position", EPS); + EXPECT_EQ(TARGET, position); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; }