fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 06:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionSmoothingTest: remove duplicate vector comparison

Browse Source
This commit is contained in:
Matthias Grob
2023-11-21 19:29:55 +01:00
parent 591845bb41
commit 14b8afe972
2 changed files with 6 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/PositionSmoothing.cpp
-2
View File
@@ -35,8 +35,6 @@
#include "TrajectoryConstraints.hpp"
#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <matrix/matrix/helper_functions.hpp>
void PositionSmoothing::_generateSetpoints(
const Vector3f &position,
src/lib/motion_planning/PositionSmoothingTest.cpp
+6 -26
View File
@@ -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";
}