bench_v1.cpp

#include <pathfinder.h>
#include "benchmark/benchmark.h"

static void BM_BenchmarkPathfinder(benchmark::State &state) {
  Waypoint points[2];
  points[0] = Waypoint{2, 2, 0};
  points[1] = Waypoint{5, 5, d2r(45)};

  double loop_time = 1.0 / static_cast<double>(state.range(0));

  int num_iter = 0;
  int num_gens = 0;
  double realtime = 0;

  for (auto _ : state) {
    state.PauseTiming();
    TrajectoryCandidate candidate;
    state.ResumeTiming();

    pathfinder_prepare(points, 2, FIT_HERMITE_CUBIC, PATHFINDER_SAMPLES_LOW, loop_time, 3.3, 5, 60, &candidate);
    
    state.PauseTiming();
    Segment *traj = (Segment *)malloc(candidate.length * sizeof(Segment));
    Segment *left = (Segment *)malloc(candidate.length * sizeof(Segment));
    Segment *right = (Segment *)malloc(candidate.length * sizeof(Segment));
    state.ResumeTiming();

    pathfinder_generate(&candidate, traj);
    pathfinder_modify_tank(traj, candidate.length, left, right, 0.5);

    num_gens += candidate.length;
    realtime += candidate.length * loop_time;
    num_iter++;
  }

  state.counters["NumStates"] = num_gens / num_iter;
  state.counters["LoopDt"] = loop_time;
  state.counters["MPExecTime"] = realtime / num_iter;
}

BENCHMARK(BM_BenchmarkPathfinder)->Arg(10)->Arg(100)->Arg(1000)->Unit(benchmark::kMillisecond);

BENCHMARK_MAIN();