From fece40c09fc1c83c118c23339d2072766f8e14bc Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Thu, 28 Nov 2024 19:10:23 -0700
Subject: [PATCH] Show error message when data logger fails to write

* I observed the benchmarks failing to write the output csv, but no
  error reported
* Now, an error is reported if it fails to write the data

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
---
 terrain_navigation/src/data_logger.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/terrain_navigation/src/data_logger.cpp b/terrain_navigation/src/data_logger.cpp
index 1f57e0e1..5c524754 100644
--- a/terrain_navigation/src/data_logger.cpp
+++ b/terrain_navigation/src/data_logger.cpp
@@ -41,6 +41,7 @@
 
 #include <fstream>
 #include <iostream>
+#include <cstring>
 
 DataLogger::DataLogger() {}
 
@@ -53,6 +54,10 @@ void DataLogger::writeToFile(const std::string path) {
   std::cout << "[DataLogger] Writing data to file! " << path << std::endl;
   std::ofstream output_file;
   output_file.open(path, std::ios::trunc);
+  if (output_file.fail()) {
+    std::cerr << "[DataLogger]: Failed to write to file: " << path << " with error " << strerror(errno) << std::endl;
+    return;
+  }
   if (print_header_) {
     for (auto key : keys_) {
       output_file << key << field_seperator;