Skip to content

Commit

Permalink
Unswap pose estimator 3d and 2d tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@KangarooKoala
KangarooKoala committed Oct 16, 2024
1 parent 825c618 commit 021ed00
Show file tree
Hide file tree
Showing 6 changed files with 80 additions and 80 deletions.
24 changes: 12 additions & 12 deletions wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include <wpi/print.h>

#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
#include "frc/estimator/DifferentialDrivePoseEstimator3d.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
Expand All @@ -23,7 +23,7 @@

void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator& estimator,
frc::DifferentialDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
Expand Down Expand Up @@ -147,10 +147,10 @@ void testFollowTrajectory(
}
}

TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator estimator{
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};

Expand All @@ -172,10 +172,10 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
100_ms, 250_ms, true, false);
}

TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator estimator{
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};

Expand Down Expand Up @@ -212,15 +212,15 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
}
}

TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator estimator{
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics,
frc::Rotation2d{},
0_m,
Expand Down Expand Up @@ -268,10 +268,10 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
}

TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};

frc::DifferentialDrivePoseEstimator estimator{
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

Expand All @@ -296,9 +296,9 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
1e-6);
}

TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
frc::DifferentialDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};

Expand Down
24 changes: 12 additions & 12 deletions wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include <wpi/print.h>

#include "frc/StateSpaceUtil.h"
#include "frc/estimator/DifferentialDrivePoseEstimator3d.h"
#include "frc/estimator/DifferentialDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/kinematics/DifferentialDriveKinematics.h"
Expand All @@ -23,7 +23,7 @@

void testFollowTrajectory(
const frc::DifferentialDriveKinematics& kinematics,
frc::DifferentialDrivePoseEstimator3d& estimator,
frc::DifferentialDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
Expand Down Expand Up @@ -147,10 +147,10 @@ void testFollowTrajectory(
}
}

TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator3d estimator{
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};

Expand All @@ -172,10 +172,10 @@ TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
100_ms, 250_ms, true, false);
}

TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator3d estimator{
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}};

Expand Down Expand Up @@ -212,15 +212,15 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
}
}

TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};

frc::DifferentialDrivePoseEstimator3d estimator{
frc::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
0_m,
Expand Down Expand Up @@ -268,10 +268,10 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
}

TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};

frc::DifferentialDrivePoseEstimator3d estimator{
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

Expand All @@ -296,9 +296,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
1e-6);
}

TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator3d estimator{
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};

Expand Down
32 changes: 16 additions & 16 deletions wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
#include <gtest/gtest.h>
#include <wpi/print.h>

#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/estimator/MecanumDrivePoseEstimator3d.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"

void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator& estimator,
frc::MecanumDrivePoseEstimator3d& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
Expand Down Expand Up @@ -138,16 +138,16 @@ void testFollowTrajectory(
}
}

TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, wheelPositions,
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
Expand All @@ -167,16 +167,16 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
100_ms, 250_ms, true, false);
}

TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, wheelPositions,
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};

frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
Expand Down Expand Up @@ -211,7 +211,7 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
}
}

TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
Expand All @@ -223,7 +223,7 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator estimator{
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
Expand Down Expand Up @@ -267,12 +267,12 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
}
}

TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDrivePoseEstimator estimator{
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

Expand All @@ -298,11 +298,11 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
1e-6);
}

TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};

Expand Down
32 changes: 16 additions & 16 deletions wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
#include <gtest/gtest.h>
#include <wpi/print.h>

#include "frc/estimator/MecanumDrivePoseEstimator3d.h"
#include "frc/estimator/MecanumDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/MecanumDriveKinematics.h"
#include "frc/trajectory/TrajectoryGenerator.h"

void testFollowTrajectory(
const frc::MecanumDriveKinematics& kinematics,
frc::MecanumDrivePoseEstimator3d& estimator,
frc::MecanumDrivePoseEstimator& estimator,
const frc::Trajectory& trajectory,
std::function<frc::ChassisSpeeds(frc::Trajectory::State&)>
chassisSpeedsGenerator,
Expand Down Expand Up @@ -138,16 +138,16 @@ void testFollowTrajectory(
}
}

TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, wheelPositions,
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
Expand All @@ -167,16 +167,16 @@ TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
100_ms, 250_ms, true, false);
}

TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator3d estimator{
kinematics, frc::Rotation2d{}, wheelPositions,
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
frc::MecanumDrivePoseEstimator estimator{kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};

frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
Expand Down Expand Up @@ -211,7 +211,7 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
}
}

TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements applied at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
Expand All @@ -223,7 +223,7 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {

frc::MecanumDriveWheelPositions wheelPositions;

frc::MecanumDrivePoseEstimator3d estimator{
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{},
wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
Expand Down Expand Up @@ -267,12 +267,12 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
}
}

TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};

frc::MecanumDrivePoseEstimator3d estimator{
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};

Expand All @@ -298,11 +298,11 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
1e-6);
}

TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) {
TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator3d estimator{
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};

Expand Down
Loading

0 comments on commit 021ed00

Please sign in to comment.