From d4fc37c7a3dbf7c9115bc42f8e7fecad1d9a9c75 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Wed, 26 Jun 2019 14:47:47 -0400 Subject: [PATCH] Add benchmarks --- .gitmodules | 3 +++ build.gradle | 25 ++++++++++++++++++ libs/benchmark | 1 + libs/build.gradle | 26 +++++++++++++++++++ src/bench/bench_arc_param.cpp | 14 ++++++++++ src/bench/bench_trajectory_generation.cpp | 15 +++++++++++ src/bench/main.cpp | 4 +++ .../mathematics/control/TrajectoryTracker.h | 4 +-- .../mathematics/spline/SplineGenerator.h | 2 ++ .../trajectory/TrajectoryGenerator.h | 2 +- 10 files changed, 93 insertions(+), 3 deletions(-) create mode 160000 libs/benchmark create mode 100644 src/bench/bench_arc_param.cpp create mode 100644 src/bench/bench_trajectory_generation.cpp create mode 100644 src/bench/main.cpp diff --git a/.gitmodules b/.gitmodules index 144d61c..0df926c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "libs/googletest"] path = libs/googletest url = https://github.com/google/googletest +[submodule "libs/benchmark"] + path = libs/benchmark + url = https://github.com/google/benchmark diff --git a/build.gradle b/build.gradle index 20c6df3..254f88a 100644 --- a/build.gradle +++ b/build.gradle @@ -25,6 +25,31 @@ model { } } testSuites { + falconlibraryBench(GoogleTestTestSuiteSpec) { + testing $.components.falconlibrary + + binaries.all { + if (toolChain instanceof GccCompatibleToolChain) { + linker.args << '-pthread' + } else { + // MSVC + linker.args << 'shlwapi.lib' + } + if (!project.hasProperty("withBench")) + tasks.withType(RunTestExecutable) { RunTestExecutable task -> + task.enabled = false + } + } + + sources.cpp { + source { + srcDir 'src/bench' + include '**/*.cpp' + } + lib project: ':libs', library: 'googleBench', linkage: 'static' + } + } + falconlibraryTest(GoogleTestTestSuiteSpec) { testing $.components.falconlibrary diff --git a/libs/benchmark b/libs/benchmark new file mode 160000 index 0000000..04a9343 --- /dev/null +++ b/libs/benchmark @@ -0,0 +1 @@ +Subproject commit 04a9343fc9b7886591c65933b94d0cee7a915452 diff --git a/libs/build.gradle b/libs/build.gradle index bd5be67..92fa5d0 100644 --- a/libs/build.gradle +++ b/libs/build.gradle @@ -2,6 +2,7 @@ apply plugin: "cpp" ext.libroot = new File(rootProject.rootDir, "libs") ext.gtest_root = new File(libroot, "googletest/googletest") +ext.gbench_root = new File(libroot, "benchmark") model { components { @@ -17,5 +18,30 @@ model { } } } + googleBench(NativeLibrarySpec) { + binaries.all { + if (toolChain instanceof GccCompatibleToolChain) { + cppCompiler.args << '-std=c++14' << '-Wno-deprecated-declarations' + linker.args << '-pthread' + } else { + // MSVC + cppCompiler.args << '/std:c++14' + linker.args << 'shlwapi.lib' + } + } + + sources.cpp { + source { + srcDir new File(gbench_root, "src") + include "**/*.cc" + exclude "**/benchmark_main.cc" + } + exportedHeaders { + srcDirs new File(gbench_root, "src"), new File(gbench_root, "include") + include "**/*.hpp", "**/*.h" + } + lib project: ':libs', library: "googleTest", linkage: "static" + } + } } } \ No newline at end of file diff --git a/src/bench/bench_arc_param.cpp b/src/bench/bench_arc_param.cpp new file mode 100644 index 0000000..3e07599 --- /dev/null +++ b/src/bench/bench_arc_param.cpp @@ -0,0 +1,14 @@ +#include +#include + +static void BM_ArcParam(benchmark::State& state) { + const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}; + const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}; + const auto spline = std::make_shared(initial, final); + + for (auto _ : state) { + frc5190::SplineGenerator::ParameterizeSpline(spline, 2.0, 0.05, 0.1); + } +} + +BENCHMARK(BM_ArcParam)->Arg(100)->Complexity()->Unit(benchmark::kMillisecond); \ No newline at end of file diff --git a/src/bench/bench_trajectory_generation.cpp b/src/bench/bench_trajectory_generation.cpp new file mode 100644 index 0000000..be57a69 --- /dev/null +++ b/src/bench/bench_trajectory_generation.cpp @@ -0,0 +1,15 @@ +#include +#include + +static void BM_TrajectoryGeneration(benchmark::State& state) { + const frc5190::Pose2d initial{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)}; + const frc5190::Pose2d final{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)}; + + for (auto _ : state) { + frc5190::TrajectoryGenerator::GenerateTrajectory( + std::vector{initial, final}, + std::vector*>{}, 0.0, 0.0, 3., 2., false); + } +} + +BENCHMARK(BM_TrajectoryGeneration)->Arg(100)->Complexity()->Unit(benchmark::kMillisecond); \ No newline at end of file diff --git a/src/bench/main.cpp b/src/bench/main.cpp new file mode 100644 index 0000000..a10c74f --- /dev/null +++ b/src/bench/main.cpp @@ -0,0 +1,4 @@ +#include + +BENCHMARK_MAIN(); + diff --git a/src/include/mathematics/control/TrajectoryTracker.h b/src/include/mathematics/control/TrajectoryTracker.h index ad06120..c123685 100644 --- a/src/include/mathematics/control/TrajectoryTracker.h +++ b/src/include/mathematics/control/TrajectoryTracker.h @@ -1,6 +1,6 @@ #pragma once -#include "../geometry/Pose2dWithCurvature.h" -#include "../trajectory/TimedTrajectory.h" + +#include "mathematics/trajectory/TimedTrajectory.h" #include diff --git a/src/include/mathematics/spline/SplineGenerator.h b/src/include/mathematics/spline/SplineGenerator.h index dfa2652..6b47137 100644 --- a/src/include/mathematics/spline/SplineGenerator.h +++ b/src/include/mathematics/spline/SplineGenerator.h @@ -1,6 +1,8 @@ #pragma once #include +#include + #include "../geometry/Pose2dWithCurvature.h" #include "ParametricSpline.h" diff --git a/src/include/mathematics/trajectory/TrajectoryGenerator.h b/src/include/mathematics/trajectory/TrajectoryGenerator.h index 650a894..ef4ba8d 100644 --- a/src/include/mathematics/trajectory/TrajectoryGenerator.h +++ b/src/include/mathematics/trajectory/TrajectoryGenerator.h @@ -28,7 +28,7 @@ class TrajectoryGenerator { } } - const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 0.051, 0.00127, 0.1); + const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1); auto points = indexed_trajectory.Points();