Navigator benchmark framework

lib/core/logger.cpp

@@ -4,10 +4,10 @@
 
 namespace hyped::core {
 
-Logger::Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source_)
+Logger::Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source)
     : label_(label),
       level_(level),
-      time_source_(time_source_)
+      time_source_(time_source)
 {
 }
 
@@ -47,8 +47,8 @@ void Logger::log(const LogLevel level, const char *format, ...)
         file = stderr;
         printHead(file, "FATAL");
         break;
-      default:
-        break;
+      case LogLevel::kNone:
+        return;
     }
     va_list args;
     va_start(args, format);

lib/core/logger.hpp

@@ -36,7 +36,7 @@ class ILogger {
 
 class Logger : public ILogger {
  public:
-  Logger(const char *const label, const LogLevel level, const core::ITimeSource &timer);
+  Logger(const char *const label, const LogLevel level, const core::ITimeSource &time_source);
 
   void log(const LogLevel level, const char *format, ...);
 

lib/core/time.cpp

@@ -0,0 +1,25 @@
+#include "time.hpp"
+
+namespace hyped::core {
+
+TimePoint timePointFromSecondsSinceEpoch(const std::uint64_t num_seconds_since_epoch)
+{
+  return std::chrono::system_clock::time_point(std::chrono::seconds(num_seconds_since_epoch));
+}
+
+TimePoint timePointFromNanosSinceEpoch(const std::uint64_t num_seconds_since_epoch)
+{
+#ifdef __linux__
+  return std::chrono::system_clock::time_point(std::chrono::nanoseconds(num_seconds_since_epoch));
+#else
+  return std::chrono::system_clock::time_point(
+    std::chrono::microseconds(num_seconds_since_epoch / 1000));
+#endif
+}
+
+Duration durationFromSeconds(const std::uint64_t num_seconds)
+{
+  return num_seconds * kOneSecond;
+}
+
+}  // namespace hyped::core

lib/core/time.hpp

@@ -8,11 +8,19 @@ namespace hyped::core {
  */
 using TimePoint = std::chrono::system_clock::time_point;
 
+TimePoint timePointFromSecondsSinceEpoch(const std::uint64_t num_seconds_since_epoch);
+
+TimePoint timePointFromNanosSinceEpoch(const std::uint64_t num_seconds_since_epoch);
+
 /**
  * @brief Difference between points in time; nanosecond precision.
  */
 using Duration = std::chrono::system_clock::duration;
 
+static constexpr Duration kOneSecond = static_cast<Duration>(1'000'000'000);
+
+Duration durationFromSeconds(const std::uint64_t num_seconds);
+
 /**
  * @brief Time provider allowing the user to obtain the current time of the
  * system. We abtract this away instead of using `WallClock` directly in order to
@@ -23,4 +31,4 @@ class ITimeSource {
   virtual TimePoint now() const = 0;
 };
 
-}  // namespace hyped::core
+}  // namespace hyped::core

lib/core/timer.cpp

@@ -2,16 +2,22 @@
 
 namespace hyped::core {
 
-Timer::Timer(const ITimeSource &time) : time_(time)
+Timer::Timer(const ITimeSource &time_source)
+    : time_source_(time_source),
+      time_started_(time_source.now())
 {
 }
 
-Duration Timer::measure_execution_time(const std::function<void(void)> task)
+Duration Timer::elapsed() const
 {
-  const auto before = time_.now();
-  task();
-  const auto after = time_.now();
-  return after - before;
+  return time_source_.now() - time_started_;
+}
+
+Duration Timer::reset()
+{
+  const auto previous_time_started = time_started_;
+  time_started_                    = time_source_.now();
+  return time_started_ - previous_time_started;
 }
 
 }  // namespace hyped::core

lib/core/timer.hpp

@@ -8,11 +8,13 @@ namespace hyped::core {
 
 class Timer {
  public:
-  Timer(const ITimeSource &time);
-  Duration measure_execution_time(const std::function<void(void)> task);
+  Timer(const ITimeSource &time_source);
+  Duration elapsed() const;
+  Duration reset();
 
  private:
-  const ITimeSource &time_;
+  const ITimeSource &time_source_;
+  TimePoint time_started_;
 };
 
 }  // namespace hyped::core

lib/core/types.hpp

@@ -23,7 +23,6 @@ struct CanFrame {
   std::array<std::uint8_t, 8> data;
 };
 
-// current trajectory struct
 struct Trajectory {
   Float displacement;
   Float velocity;
@@ -36,31 +35,25 @@ static constexpr std::uint8_t kNumAxis           = 3;
 static constexpr std::uint8_t kNumEncoders       = 4;
 static constexpr std::uint8_t kNumKeyence        = 2;
 
-// data format for raw sensor data
-using RawAccelerometerData = std::array<std::array<Float, kNumAxis>, kNumAccelerometers>;
-using AccelerometerData    = std::array<Float, kNumAccelerometers>;
-using EncoderData          = std::array<std::uint32_t, kNumEncoders>;
-using KeyenceData          = std::array<std::uint32_t, kNumKeyence>;
-
-// data produced by the accelerometer sensor
-// !! the values are in mg !!
-struct RawAccelerationData {
-  RawAccelerationData(const std::int32_t x,
-                      const std::int32_t y,
-                      const std::int32_t z,
-                      const TimePoint measured_at)
-      : x(x),
-        y(y),
-        z(z),
-        measured_at(measured_at)
+template<typename T>
+struct Measurement {
+  Measurement(const TimePoint &measured_at, const T &value) : measured_at(measured_at), value(value)
   {
   }
 
-  const std::int32_t x;
-  const std::int32_t y;
-  const std::int32_t z;
-  const TimePoint measured_at;
+  TimePoint measured_at;
+  T value;
+};
+
+struct RawAcceleration {
+  std::int32_t x;  // mg
+  std::int32_t y;  // mg
+  std::int32_t z;  // mg
 };
+using RawAccelerometerData         = Measurement<RawAcceleration>;
+using CombinedRawAccelerometerData = Measurement<std::array<RawAcceleration, kNumAccelerometers>>;
+using RawEncoderData               = Measurement<std::array<std::uint32_t, kNumEncoders>>;
+using RawKeyenceData               = Measurement<std::array<std::uint32_t, kNumKeyence>>;
 
 enum class Axis { kX = 0, kY, kZ };
 

lib/debug/repl.cpp

@@ -228,7 +228,7 @@ void Repl::addCommand(const Command &cmd)
 
 void Repl::addQuitCommand()
 {
-  addCommand(Command{"quit", "Quit the REPL", [this]() { exit(0); }});
+  addCommand(Command{"quit", "Quit the REPL", []() { exit(0); }});
 }
 
 void Repl::addHelpCommand()
@@ -340,7 +340,7 @@ void Repl::addPwmCommands(const std::uint8_t module)
     std::stringstream description;
     description << "Run PWM module: " << static_cast<int>(pwm_module);
     pwm_run_command.description = description.str();
-    pwm_run_command.handler     = [this, pwm, pwm_module]() {
+    pwm_run_command.handler     = [this, pwm]() {
       std::uint32_t period;
       std::cout << "Period: ";
       std::cin >> period;
@@ -383,7 +383,7 @@ void Repl::addPwmCommands(const std::uint8_t module)
     std::stringstream description;
     description << "Stop PWM module: " << static_cast<int>(pwm_module);
     pwm_stop_command.description = description.str();
-    pwm_stop_command.handler     = [this, pwm, pwm_module]() {
+    pwm_stop_command.handler     = [this, pwm]() {
       const core::Result disable_result = pwm->setMode(io::Mode::kStop);
       if (disable_result == core::Result::kFailure) {
         logger_.log(core::LogLevel::kFatal, "Failed to stop PWM module");
@@ -415,7 +415,7 @@ void Repl::addSpiCommands(const std::uint8_t bus)
       std::cout << "Register address: ";
       std::cin >> register_address;
       std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
-      std::uint8_t read_buffer;
+      std::uint8_t read_buffer  = 0;
       const core::Result result = spi->read(register_address, &read_buffer, 1);
       if (result == core::Result::kSuccess) {
         logger_.log(core::LogLevel::kDebug, "SPI value from bus %d: %d", bus, read_buffer);
@@ -466,7 +466,8 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
                 "Asking for accelerometer on another address as what is hard coded");
     return;
   }
-  const auto accelerometer = std::make_shared<sensors::Accelerometer>(logger_, *i2c, bus);
+  const auto accelerometer
+    = std::make_shared<sensors::Accelerometer>(logger_, wall_clock_, *i2c, bus);
   accelerometer->configure();
   Command accelerometer_read_command;
   std::stringstream identifier;
@@ -478,16 +479,16 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
               << "I2C bus " << static_cast<int>(bus);
   accelerometer_read_command.description = description.str();
   accelerometer_read_command.handler     = [this, accelerometer, bus]() {
-    const auto value = accelerometer->read();
-    if (!value) {
+    const auto acceleration = accelerometer->read();
+    if (!acceleration) {
       logger_.log(core::LogLevel::kFatal, "Failed to read accelerometer from bus %d", bus);
     } else {
-      const core::RawAccelerationData result = value.value();
+      // TODOLater: Print timestamp
       logger_.log(core::LogLevel::kInfo,
                   "Acceleration in mg: \n x %d \n y %d \n z %d",
-                  result.x,
-                  result.y,
-                  result.z);
+                  acceleration->value.x,
+                  acceleration->value.y,
+                  acceleration->value.z);
     }
   };
   addCommand(accelerometer_read_command);

lib/debug/repl.hpp

@@ -7,6 +7,7 @@
 #include <vector>
 
 #include <core/logger.hpp>
+#include <core/wall_clock.hpp>
 #include <io/hardware_adc.hpp>
 #include <io/hardware_gpio.hpp>
 #include <io/hardware_i2c.hpp>
@@ -44,7 +45,8 @@ class Repl {
   void addUartCommands(const std::uint8_t bus);
 
   core::ILogger &logger_;
+  core::WallClock wall_clock_;
   std::map<std::string, Command> command_map_;
 };
 
-}  // namespace hyped::debug
+}  // namespace hyped::debug

lib/navigation/CMakeLists.txt

@@ -2,9 +2,11 @@ set(target hyped_navigation)
 file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
 file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
 add_library(${target} STATIC ${headers} ${code})
+find_package (Eigen3 REQUIRED NO_MODULE)
 include_directories(${target}
     INTERFACE
     "${CMAKE_SOURCE_DIR}/lib"
     "${CMAKE_CURRENT_SOURCE_DIR}"
 )
-target_link_libraries(${target} PUBLIC Eigen3::Eigen)
+target_link_libraries(${target} PUBLIC Eigen3::Eigen)
+add_subdirectory(benchmark)

lib/navigation/benchmark/CMakeLists.txt

@@ -0,0 +1,10 @@
+set(target hyped_navigation_benchmark)
+file(GLOB headers "${CMAKE_CURRENT_SOURCE_DIR}/*.hpp")
+file(GLOB code "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp")
+add_library(${target} STATIC ${headers} ${code})
+include_directories(${target}
+    INTERFACE
+    "${CMAKE_SOURCE_DIR}/lib"
+    "${CMAKE_CURRENT_SOURCE_DIR}"
+)
+target_link_libraries(${target} hyped_navigation)

lib/navigation/benchmark/benchmark.cpp

@@ -0,0 +1,66 @@
+#include "benchmark.hpp"
+
+#include <core/timer.hpp>
+
+namespace hyped::navigation::benchmark {
+
+TrajectoryError::TrajectoryError(const core::Trajectory &expected, const core::Trajectory &actual)
+    : displacement(std::abs(expected.displacement - actual.displacement)),
+      velocity(std::abs(expected.velocity - actual.velocity)),
+      acceleration(std::abs(expected.acceleration - actual.acceleration))
+{
+}
+
+Benchmark::Benchmark(const core::ITimeSource &time_source, const Data &data)
+    : time_source_(time_source),
+      data_(data),
+      relevant_times_(data.getRelevantTimes())
+{
+}
+
+// TODOLater: Allow this to operate using constructors rather than already initialised navigators.
+// This would have two benefits:
+// 1. The constraints and relations of the input arguments would be more clear.
+// 2. We could measure and compare the initialisation time taken by each of the navigators.
+std::optional<Result> Benchmark::run(utils::ManualTime &manual_time, INavigator &navigator)
+{
+  if (manual_time.now() != std::chrono::system_clock::from_time_t(0)) { return std::nullopt; }
+  const core::Timer full_benchmark_timer(time_source_);
+  Result result;
+  for (const auto &time_point : relevant_times_) {
+    const auto encoder_data = data_.getEncoderDataAt(time_point);
+    if (encoder_data) {
+      const core::Timer encoder_data_timer(time_source_);
+      navigator.encoderUpdate(*encoder_data);
+      result.time_taken_for_encoder_data.push_back(encoder_data_timer.elapsed());
+    }
+    const auto acceleration_data = data_.getAccelerationDataAt(time_point);
+    if (acceleration_data) {
+      const core::Timer acceleration_data_timer(time_source_);
+      navigator.accelerometerUpdate(*acceleration_data);
+      result.time_taken_for_acceleration_data.push_back(acceleration_data_timer.elapsed());
+    }
+    const auto keyence_data = data_.getKeyenceDataAt(time_point);
+    if (keyence_data) {
+      const core::Timer keyence_data_timer(time_source_);
+      navigator.keyenceUpdate(*keyence_data);
+      result.time_taken_for_keyence_data.push_back(keyence_data_timer.elapsed());
+    }
+    const auto expected_trajectory = data_.getTrajectoryDataAt(time_point);
+    if (expected_trajectory) {
+      const core::Timer trajectory_timer(time_source_);
+      const auto calculated_trajectory = navigator.currentTrajectory();
+      if (calculated_trajectory) {
+        result.trajectory_errors.emplace_back(
+          TrajectoryError(*expected_trajectory, *calculated_trajectory));
+      } else {
+        result.trajectory_errors.push_back(std::nullopt);
+      }
+      result.time_taken_for_trajectory.push_back(trajectory_timer.elapsed());
+    }
+  }
+  result.total_time_taken = full_benchmark_timer.elapsed();
+  return result;
+}
+
+}  // namespace hyped::navigation::benchmark