removal of ground lock, improved redirectable debug logging

AirLib/AirLib.vcxproj

@@ -28,7 +28,6 @@
     <ClInclude Include="include\common\common_utils\ExceptionUtils.hpp" />
     <ClInclude Include="include\common\common_utils\FileSystem.hpp" />
     <ClInclude Include="include\common\common_utils\json.hpp" />
-    <ClInclude Include="include\common\common_utils\Log.hpp" />
     <ClInclude Include="include\common\common_utils\MedianFilter.hpp" />
     <ClInclude Include="include\common\common_utils\OnlineStats.hpp" />
     <ClInclude Include="include\common\common_utils\optional.hpp" />
@@ -131,7 +130,6 @@
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp" />
-    <ClCompile Include="src\controllers\Log.cpp" />
     <ClCompile Include="src\controllers\MavLinkDroneController.cpp" />
     <ClCompile Include="src\safety\ObstacleMap.cpp" />
     <ClCompile Include="src\rpc\RpcLibClient.cpp" />

AirLib/AirLib.vcxproj.filters

@@ -309,9 +309,6 @@
     <ClInclude Include="include\vehicles\configs\RosFlightQuadX.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="include\common\common_utils\Log.hpp">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="include\vehicles\MultiRotorParamsFactory.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
@@ -368,8 +365,5 @@
     <ClCompile Include="src\controllers\FileSystem.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="src\controllers\Log.cpp">
-      <Filter>Source Files</Filter>
-    </ClCompile>
   </ItemGroup>
 </Project>

AirLib/include/common/CommonStructs.hpp

@@ -41,7 +41,7 @@ struct Wrench {
     }
 
     //support basic arithmatic 
-    Wrench operator+(const Wrench& other)
+    Wrench operator+(const Wrench& other) const
     {
         Wrench result;
         result.force = this->force + other.force;
@@ -54,7 +54,7 @@ struct Wrench {
         torque += other.torque;
         return *this;
     }
-    Wrench operator-(const Wrench& other)
+    Wrench operator-(const Wrench& other) const
     {
         Wrench result;
         result.force = this->force - other.force;
@@ -174,11 +174,17 @@ struct GeoPoint {
 
 struct CollisionInfo {
     bool has_collided = false;
-    int collison_count = 0;
     Vector3r normal = Vector3r::Zero();
     Vector3r impact_point = Vector3r::Zero();
     Vector3r position = Vector3r::Zero();
     real_T penetration_depth = 0;
+    TTimePoint time_stamp = 0;
+};
+
+struct CollisonResponseInfo {
+    unsigned int collison_count_raw = 0;
+    unsigned int collison_count_non_resting = 0;
+    TTimePoint collision_time_stamp = 0;
 };
 
 struct GeoPose {

AirLib/include/common/common_utils/FileSystem.hpp

@@ -22,12 +22,12 @@ class FileSystem
 
 public:
 
-	// please use the combine() method instead.
-	static const char kPathSeparator =
+    // please use the combine() method instead.
+    static const char kPathSeparator =
 #ifdef _WIN32
-		'\\';
+        '\\';
 #else
-		'/';
+        '/';
 #endif
 
     static std::string createDirectory(std::string fullPath);
@@ -69,13 +69,13 @@ class FileSystem
         return parentFolder + kPathSeparator + child;
     }
 
-	static void removeLeaf(std::string& path) {
-		size_t size = path.size();
-		size_t pos = path.find_last_of('/');
-		if (pos != std::string::npos) {
-			path.erase(pos, size - pos);
-		}
-	}
+    static void removeLeaf(std::string& path) {
+        size_t size = path.size();
+        size_t pos = path.find_last_of('/');
+        if (pos != std::string::npos) {
+            path.erase(pos, size - pos);
+        }
+    }
 
 
     static std::string getFileExtension(const std::string str)
@@ -148,7 +148,7 @@ class FileSystem
         std::string filepath = getLogFileNamePath("log_", suffix, ".tsv", true);
         createTextFile(filepath, flog);
 
-        Utils::logMessage("log file started: %s", filepath.c_str());
+        Utils::log(Utils::stringf("log file started: %s", filepath.c_str()));
         flog.exceptions(flog.exceptions() | std::ios::failbit | std::ifstream::badbit);
         return filepath;
     }

AirLib/include/common/common_utils/Utils.hpp

@@ -21,7 +21,6 @@
 #include <iostream>
 #include <limits>
 #include <queue>
-#include "Log.hpp"
 #include "type_utils.hpp"
 
 #ifndef _WIN32
@@ -98,7 +97,17 @@ class Utils {
 
 
 public:
-
+    class Logger {
+    public:
+        virtual void log(int level, const std::string& message)
+        {
+            if (level >= 0)
+                std::cout << message;
+            else
+                std::cerr << message;
+        }
+    };
+
     static void enableImmediateConsoleFlush() {
         //disable buffering
         setbuf(stdout, NULL);
@@ -126,42 +135,22 @@ class Utils {
         return static_cast<float>(radians * 180.0f / M_PI);
     }
 
-    static void logMessage(const char* format, ...) {
-        va_list args;
-        va_start(args, format);
-
-        auto size = _vscprintf(format, args) + 1U;
-        std::unique_ptr<char[]> buf(new char[size]);
+    static Logger* getSetLogger(Logger* logger = nullptr)
+    {
+        static Logger logger_default_;
+        static Logger* logger_;
 
-#ifndef _MSC_VER
-        IGNORE_FORMAT_STRING_ON
-        vsnprintf(buf.get(), size, format, args);
-        IGNORE_FORMAT_STRING_OFF
-#else
-        vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
-#endif
-        va_end(args);
+        if (logger != nullptr)
+            logger_ = logger;
+        else if (logger_ == nullptr)
+            logger_ = &logger_default_;
 
-        Log::getLog()->logMessage(buf.get());
+        return logger_;
     }
 
-    static void logError(const char* format, ...) {
-        va_list args;
-        va_start(args, format);
-
-        auto size = _vscprintf(format, args) + 1U;
-        std::unique_ptr<char[]> buf(new char[size]);
-
-#ifndef _MSC_VER
-        IGNORE_FORMAT_STRING_ON
-        vsnprintf(buf.get(), size, format, args);
-        IGNORE_FORMAT_STRING_OFF
-#else
-        vsnprintf_s(buf.get(), size, _TRUNCATE, format, args);
-#endif
-        va_end(args);
-
-        Log::getLog()->logError(buf.get());
+    static void log(std::string message, int level = 0)
+    {
+        getSetLogger()->log(level, message);
     }
 
     template <typename T>

AirLib/include/common/common_utils/WorkerThread.hpp

@@ -216,7 +216,7 @@ namespace msr {
                             pending->execute();
                         }
                         catch (std::exception& e) {
-                            common_utils::Utils::logMessage("WorkerThread caught unhandled exception: %s", e.what());
+                            Utils::log(Utils::stringf("WorkerThread caught unhandled exception: %s", e.what()));
                         }
                         // we use cancel here to communicate to enqueueAndWait that the task is complete.
                         pending->complete();