Add robust logging framework

include/scrimmage/entity/Entity.h

@@ -192,13 +192,10 @@ class Entity : public std::enable_shared_from_this<Entity> {
 
     PubSubPtr& pubsub() { return pubsub_; }
 
-    PrintPtr& printer() { return printer_; }
-
     const ParameterServerPtr& param_server() { return param_server_; }
 
     double radius() { return radius_; }
     void set_time_ptr(TimePtr t);
-    void set_printer(PrintPtr printer);
     void set_gpu_controller(GPUControllerPtr gpu_controller);
     GPUControllerPtr gpu_controller();
     bool using_gpu_motion_model() const;
@@ -245,7 +242,6 @@ class Entity : public std::enable_shared_from_this<Entity> {
     FileSearchPtr file_search_;
     GPUControllerPtr gpu_controller_;
     PubSubPtr pubsub_;
-    PrintPtr printer_;
     GPUControllerPtr gpu_;
     GlobalServicePtr global_services_;
     ParameterServerPtr param_server_;

include/scrimmage/entity/EntityPluginHelper.h

@@ -45,6 +45,7 @@
 
 #include "scrimmage/autonomy/Autonomy.h"
 #include "scrimmage/fwd_decl.h"
+#include "scrimmage/log/Logger.h"
 #include "scrimmage/motion/Controller.h"
 #include "scrimmage/parse/ConfigParse.h"
 #include "scrimmage/plugin_manager/PluginManager.h"
@@ -82,9 +83,9 @@ boost::optional<std::shared_ptr<T>> make_autonomy(
         overrides,
         plugin_tags);
     if (status.status == PluginStatus<T>::cast_failed) {
-        std::cout << "Failed to open autonomy plugin: " << autonomy_name << std::endl;
+        LOG_ERROR("Failed to open autonomy plugin: " << autonomy_name);
     } else if (status.status == PluginStatus<T>::parse_failed) {
-        std::cout << "Parsing of plugin failed: " << autonomy_name << std::endl;
+        LOG_ERROR("Failed to parse autonomy plugin config: " << autonomy_name);
     } else if (status.status == PluginStatus<T>::loaded) {
         std::shared_ptr<T> autonomy = status.plugin;
         // Connect the autonomy to the first controller
@@ -106,17 +107,30 @@ boost::optional<std::shared_ptr<T>> make_autonomy(
         param_override_func(config_parse.params());
 
         if (debug_level > 1) {
-            std::cout << "--------------------------------" << std::endl;
-            std::cout << "Autonomy plugin params: " << autonomy_name << std::endl;
-            std::cout << config_parse;
+            LOG_INFO("--------------------------------");
+            LOG_INFO("Autonomy plugin params: " << autonomy_name);
+            LOG_INFO(config_parse);
+        }
+        try {
+            autonomy->init(config_parse.params());
+        } catch (const std::exception& e) {
+            LOG_ERROR("Autonomy plugin '" << autonomy_name << "' threw exception during init: " << e.what());
+            return boost::none;
+        } catch (...) {
+            LOG_ERROR("Autonomy plugin '" << autonomy_name << "' threw unknown exception during init");
+            return boost::none;
         }
-        autonomy->init(config_parse.params());
 
         // get loop rate from plugin's params
         auto it_loop_rate = config_parse.params().find("loop_rate");
         if (it_loop_rate != config_parse.params().end()) {
-            const double loop_rate = std::stod(it_loop_rate->second);
-            autonomy->set_loop_rate(loop_rate);
+            try {
+                const double loop_rate = std::stod(it_loop_rate->second);
+                autonomy->set_loop_rate(loop_rate);
+            } catch (const std::exception& e) {
+                LOG_ERROR("Autonomy plugin '" << autonomy_name << "': invalid loop_rate '" << it_loop_rate->second << "': " << e.what());
+                return boost::none;
+            }
         }
         return boost::optional<std::shared_ptr<T>>{autonomy};
     }

include/scrimmage/entity/External.h

@@ -54,7 +54,6 @@
 #include "scrimmage/common/Time.h"
 #include "scrimmage/entity/Entity.h"
 #include "scrimmage/log/Log.h"
-#include "scrimmage/log/Print.h"
 #include "scrimmage/math/State.h"
 #include "scrimmage/motion/Controller.h"
 #include "scrimmage/proto/ProtoConversions.h"
@@ -135,7 +134,6 @@ class External {
     std::shared_ptr<Log> log_;
     double last_t_;
     PubSubPtr pubsub_;
-    PrintPtr printer_;
     TimePtr time_;
     ParameterServerPtr param_server_;
     GlobalServicePtr global_services_;

include/scrimmage/fwd_decl.h

@@ -83,9 +83,6 @@ using EntityPtr = std::shared_ptr<Entity>;
 class Time;
 using TimePtr = std::shared_ptr<Time>;
 
-class Print;
-using PrintPtr = std::shared_ptr<Print>;
-
 class ParameterBase;
 using ParameterBasePtr = std::shared_ptr<ParameterBase>;
 

include/scrimmage/log/Logger.h

@@ -0,0 +1,160 @@
+/*
+ * ---------------------------------------------------------------------------
+ * @section LICENSE
+ *
+ * Copyright (c) 2020 Georgia Tech Research Institute (GTRI)
+ *               All Rights Reserved
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ * ---------------------------------------------------------------------------
+ * @file Logger.h
+ * @author Ethan M Boos <[email protected]>
+ * @version 1.0
+ * ---------------------------------------------------------------------------
+ * @brief Simple thread-safe logging with file/line capture.
+ *
+ * Usage with Logger class:
+ *   scrimmage::Logger& log = scrimmage::Logger::instance();
+ *   log.init_dir("/path/to/logs");  // creates scrimmage.log in that dir
+ *   log.info("message", __FILE__, __LINE__);
+ *
+ * Usage with macros (recommended - auto-captures file/line):
+ *   LOG_INFO("status update");
+ *   LOG_INFO("value is " << x << " units");   // stream syntax for variables
+ *   LOG_WARN("entity " << id << " has low health");
+ *   LOG_ERROR("error message");
+ *   LOG_ERROR("parse failed", exception);     // appends ": <exception.what()>"
+ *
+ * ---------------------------------------------------------------------------
+ */
+
+#ifndef INCLUDE_SCRIMMAGE_LOG_LOGGER_H_
+#define INCLUDE_SCRIMMAGE_LOG_LOGGER_H_
+
+#include <exception>
+#include <fstream>
+#include <iostream>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace scrimmage {
+
+/**
+ * @brief Thread-safe singleton logger with file and console output.
+ */
+class Logger {
+ public:
+    /// Get the singleton instance
+    static Logger& instance() {
+        static Logger logger;
+        return logger;
+    }
+
+    /// Initialize logging to scrimmage.log in the given directory
+    void init_dir(const std::string& log_dir);
+
+    /// Close the log file
+    void close();
+
+    /// Log an INFO message
+    void info(const std::string& msg, const char* file = "", int line = 0);
+
+    /// Log a WARN message
+    void warn(const std::string& msg, const char* file = "", int line = 0);
+
+    /// Log an ERROR message
+    void error(const std::string& msg, const char* file = "", int line = 0);
+
+    /// Log an ERROR message with exception details
+    void error(const std::string& msg, const std::exception& ex,
+               const char* file = "", int line = 0);
+
+    /// Log a WARN message with exception details
+    void warn(const std::string& msg, const std::exception& ex,
+              const char* file = "", int line = 0);
+
+ private:
+    Logger() = default;
+    ~Logger();
+    Logger(const Logger&) = delete;
+    Logger& operator=(const Logger&) = delete;
+
+    void init(const std::string& path);
+    void write(const char* level, const char* file, int line, const std::string& msg);
+
+    /// Buffered message for pre-init logging
+    struct PendingMessage {
+        std::string level;
+        std::string file;
+        int line;
+        std::string msg;
+        std::string formatted;  // Pre-formatted for output
+    };
+
+    std::ofstream file_;
+    std::mutex mutex_;
+    std::vector<PendingMessage> pending_;
+    bool dir_initialized_{false};
+};
+
+}  // namespace scrimmage
+
+// =============================================================================
+// Convenience macros (auto-capture file and line)
+// =============================================================================
+
+#define LOG_INFO(expr) \
+    do { \
+        std::ostringstream _ss; \
+        _ss << expr; \
+        ::scrimmage::Logger::instance().info(_ss.str(), __FILE__, __LINE__); \
+    } while (0)
+
+#define LOG_WARN_1(expr) \
+    do { \
+        std::ostringstream _ss; \
+        _ss << expr; \
+        ::scrimmage::Logger::instance().warn(_ss.str(), __FILE__, __LINE__); \
+    } while (0)
+
+#define LOG_WARN_2(expr, ex) \
+    do { \
+        std::ostringstream _ss; \
+        _ss << expr; \
+        ::scrimmage::Logger::instance().warn(_ss.str(), ex, __FILE__, __LINE__); \
+    } while (0)
+
+#define LOG_ERROR_1(expr) \
+    do { \
+        std::ostringstream _ss; \
+        _ss << expr; \
+        ::scrimmage::Logger::instance().error(_ss.str(), __FILE__, __LINE__); \
+    } while (0)
+
+#define LOG_ERROR_2(expr, ex) \
+    do { \
+        std::ostringstream _ss; \
+        _ss << expr; \
+        ::scrimmage::Logger::instance().error(_ss.str(), ex, __FILE__, __LINE__); \
+    } while (0)
+
+#define _SC_GET_MACRO(_1, _2, NAME, ...) NAME
+
+#define LOG_WARN(...) \
+    _SC_GET_MACRO(__VA_ARGS__, LOG_WARN_2, LOG_WARN_1)(__VA_ARGS__)
+
+#define LOG_ERROR(...) \
+    _SC_GET_MACRO(__VA_ARGS__, LOG_ERROR_2, LOG_ERROR_1)(__VA_ARGS__)
+
+#endif  // INCLUDE_SCRIMMAGE_LOG_LOGGER_H_