Redirect std::cout and std::cerr to ROS logging macros

README.md

@@ -8,7 +8,7 @@ independent project.
 
 ## Setup
 
-For comprehensive setup instructions, follow our [setup guide](https://ubcsailbot.github.io/docs/main/current/sailbot_workspace/setup/).
+For comprehensive setup instructions, follow our [setup guide](https://ubcsailbot.github.io/sailbot_workspace/main/current/sailbot_workspace/setup/).
 
 ## Building
 
@@ -20,7 +20,7 @@ For comprehensive setup instructions, follow our [setup guide](https://ubcsailbo
 
 ### ROS Launch
 
-[Instructions found here.](https://ubcsailbot.github.io/docs/main/current/sailbot_workspace/launch_files/)
+[Instructions found here.](https://ubcsailbot.github.io/sailbot_workspace/main/current/sailbot_workspace/launch_files/)
 
 For example:
 

functions.cmake

@@ -20,12 +20,13 @@ function(make_exe module srcs link_libs inc_dirs compile_defs)
     add_executable(${bin_module} ${srcs})
     target_compile_definitions(${bin_module} PUBLIC ${compile_defs})
     ament_target_dependencies(${bin_module} PUBLIC ${ROS_DEPS})
-    target_link_libraries(${bin_module} PUBLIC ${link_libs} boost_program_options)
+    target_link_libraries(${bin_module} PUBLIC ${link_libs} boost_program_options net_node)
     target_include_directories(
         ${bin_module} PUBLIC
         ${CMAKE_CURRENT_LIST_DIR}/inc
         ${CMAKE_SOURCE_DIR}/lib
         ${inc_dirs}
+        ${net_node_inc_dir}
     )
     add_dependencies(${bin_module} ${AUTOGEN_TARGETS})
     install(TARGETS ${bin_module} DESTINATION lib/${PROJECT_NAME})

lib/CMakeLists.txt

@@ -1,4 +1,5 @@
 add_subdirectory(cmn_hdrs)
+add_subdirectory(net_node)
 add_subdirectory(protofiles)
 add_subdirectory(sailbot_db)
 

lib/net_node/CMakeLists.txt

@@ -0,0 +1,15 @@
+set(module net_node)
+
+set(link_libs
+)
+
+set(inc_dirs
+)
+
+set(compile_defs
+)
+
+set(srcs
+    ${CMAKE_CURRENT_LIST_DIR}/src/net_node.cpp
+)
+make_lib(${module} "${srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}")

lib/net_node/inc/net_node.h

@@ -0,0 +1,53 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+
+/**
+ * Network Systems custom ROS Node.
+ * Redirects std::cout and std::cerr streams to ROS logging macros.
+ * @note An extra newline character is added to ROS logs from redirected streams. Not the prettiest, but it's harmless.
+ *       Errors are extra ugly, with multiple empty newlines. It does make them extra obvious though.
+ */
+class NetNode : public rclcpp::Node
+{
+public:
+    /**
+     * @brief Initialize a NetNode's buffers and stream redirections
+     *
+     * @param node_name Name of the node to pass to the base rclcpp::Node
+     */
+    explicit NetNode(const std::string & node_name);
+    ~NetNode();
+
+private:
+    /**
+     * @brief Custom string buffer that flushes its contents to ROS logging macros.
+     *        https://stackoverflow.com/a/14232652
+     */
+    class LogBuf : public std::stringbuf
+    {
+    public:
+        /**
+         * @brief Initialize a LogBuf for the NetNode
+         *
+         * @param fd     File descriptor. Must be either STDOUT_FILENO or STDERR_FILENO
+         * @param logger Logger object of from the encompassing NetNode
+         */
+        LogBuf(int fd, rclcpp::Logger logger);
+
+        /**
+         * @brief Called when the buffer is flushed. Invokes ROS logging macros and clears buffer.
+         *
+         * @return 0
+         */
+        virtual int sync();
+
+    private:
+        const int            fd_;      // File descriptor to redirect (either STDOUT_FILENO or STDERR_FILENO)
+        const rclcpp::Logger logger_;  // Logger instance from the encompassing NetNode
+    };
+
+    std::streambuf * og_stdout_buf_;   // Original buffer for stdout
+    std::streambuf * og_stderr_buf_;   // Original buffer for stderr
+    LogBuf           new_stdout_buf_;  // LogBuf to redirect stdout to
+    LogBuf           new_stderr_buf_;  // LogBuf to redirect stderr to
+};

lib/net_node/src/net_node.cpp

@@ -0,0 +1,42 @@
+#include "net_node.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+NetNode::NetNode(const std::string & node_name)
+: rclcpp::Node(node_name),
+  // When constructing the new buffers, create a new rclcpp::Logger with ":net_node" appended to the name so that
+  // we can identify that redirected messages are output from this file, rather than the _ros_intf.cpp file.
+  // Hopefully this will help reduce confusion with respect to the printed line numbers.
+  new_stdout_buf_(STDOUT_FILENO, rclcpp::get_logger(std::string(this->get_logger().get_name()) + ":net_node")),
+  new_stderr_buf_(STDERR_FILENO, rclcpp::get_logger(std::string(this->get_logger().get_name()) + ":net_node"))
+{
+    og_stdout_buf_ = std::cout.rdbuf();
+    og_stderr_buf_ = std::cerr.rdbuf();
+    std::cout.rdbuf(&new_stdout_buf_);
+    std::cerr.rdbuf(&new_stderr_buf_);
+}
+
+NetNode::~NetNode()
+{
+    std::cout.rdbuf(og_stdout_buf_);
+    std::cerr.rdbuf(og_stderr_buf_);
+}
+
+NetNode::LogBuf::LogBuf(int fd, rclcpp::Logger logger) : fd_(fd), logger_(logger) {}
+
+int NetNode::LogBuf::sync()
+{
+    switch (fd_) {
+        case STDOUT_FILENO:
+            RCLCPP_INFO(logger_, "%s", this->str().c_str());
+            break;
+        case STDERR_FILENO:
+            RCLCPP_ERROR(logger_, "%s", this->str().c_str());
+            break;
+        default:
+            // unreachable
+            throw std::runtime_error("Invalid file descriptor! " + std::to_string(fd_));
+    }
+    this->str("");
+    return 0;
+}

lib/sailbot_db/src/sailbot_db.cpp

@@ -63,7 +63,7 @@ bool SailbotDB::testConnection()
         db.run_command(ping_cmd.view());
         return true;
     } catch (const std::exception & e) {
-        std::cout << "Exception: " << e.what() << std::endl;
+        std::cerr << "Exception: " << e.what() << std::endl;
         return false;
     }
 }

projects/can_transceiver/src/can_transceiver_ros_intf.cpp

@@ -15,6 +15,7 @@
 #include "can_transceiver.h"
 #include "cmn_hdrs/ros_info.h"
 #include "cmn_hdrs/shared_constants.h"
+#include "net_node.h"
 
 constexpr int  QUEUE_SIZE     = 10;  // Arbitrary number
 constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500);
@@ -23,10 +24,10 @@ namespace msg = custom_interfaces::msg;
 using CAN_FP::CanFrame;
 using CAN_FP::CanId;
 
-class CanTransceiverIntf : public rclcpp::Node
+class CanTransceiverIntf : public NetNode
 {
 public:
-    CanTransceiverIntf() : Node("can_transceiver_node")
+    CanTransceiverIntf() : NetNode("can_transceiver_node")
     {
         this->declare_parameter("enabled", true);
 
@@ -188,8 +189,20 @@ class CanTransceiverIntf : public rclcpp::Node
 
 int main(int argc, char * argv[])
 {
+    bool err = false;
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<CanTransceiverIntf>());
+    try {
+        std::shared_ptr<CanTransceiverIntf> node = std::make_shared<CanTransceiverIntf>();
+        try {
+            rclcpp::spin(node);
+        } catch (std::exception & e) {
+            RCLCPP_ERROR(node->get_logger(), "%s", e.what());
+            throw e;
+        }
+    } catch (std::exception & e) {
+        std::cerr << e.what() << std::endl;
+        err = true;
+    }
     rclcpp::shutdown();
-    return 0;
+    return err ? -1 : 0;
 }

projects/example/src/cached_fib_ros_intf.cpp

@@ -1,5 +1,6 @@
 // Include this module
 #include "cached_fib.h"
+#include "net_node.h"
 // Include ROS headers
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/u_int64.hpp>
@@ -10,10 +11,10 @@ constexpr int ROS_Q_SIZE    = 10;
 constexpr int INIT_FIB_SIZE = 5;
 }  // namespace
 
-class CachedFibNode : public rclcpp::Node
+class CachedFibNode : public NetNode
 {
 public:
-    explicit CachedFibNode(const std::size_t initSize) : Node("cached_fib_node"), c_fib_(initSize)
+    explicit CachedFibNode(const std::size_t initSize) : NetNode("cached_fib_node"), c_fib_(initSize)
     {
         this->declare_parameter("enabled", false);
         bool enabled = this->get_parameter("enabled").as_bool();
@@ -50,8 +51,20 @@ class CachedFibNode : public rclcpp::Node
 
 int main(int argc, char * argv[])
 {
+    bool err = false;
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<CachedFibNode>(INIT_FIB_SIZE));
+    try {
+        std::shared_ptr<CachedFibNode> node = std::make_shared<CachedFibNode>(INIT_FIB_SIZE);
+        try {
+            rclcpp::spin(node);
+        } catch (std::exception & e) {
+            RCLCPP_ERROR(node->get_logger(), "%s", e.what());
+            throw e;
+        }
+    } catch (std::exception & e) {
+        std::cerr << e.what() << std::endl;
+        err = true;
+    }
     rclcpp::shutdown();
-    return 0;
+    return err ? -1 : 0;
 }

projects/local_transceiver/src/local_transceiver_ros_intf.cpp

@@ -9,12 +9,13 @@
 #include "cmn_hdrs/ros_info.h"
 #include "cmn_hdrs/shared_constants.h"
 #include "local_transceiver.h"
+#include "net_node.h"
 
 /**
  * Local Transceiver Interface Node
  *
  */
-class LocalTransceiverIntf : public rclcpp::Node
+class LocalTransceiverIntf : public NetNode
 {
 public:
     /**
@@ -23,7 +24,7 @@ class LocalTransceiverIntf : public rclcpp::Node
      * @param lcl_trns Local Transceiver instance
      */
     explicit LocalTransceiverIntf(std::shared_ptr<LocalTransceiver> lcl_trns)
-    : Node("local_transceiver_node"), lcl_trns_(lcl_trns)
+    : NetNode("local_transceiver_node"), lcl_trns_(lcl_trns)
     {
         static constexpr int  ROS_Q_SIZE     = 5;
         static constexpr auto TIMER_INTERVAL = std::chrono::milliseconds(500);
@@ -64,9 +65,21 @@ class LocalTransceiverIntf : public rclcpp::Node
 
 int main(int argc, char * argv[])
 {
+    bool err = false;
     rclcpp::init(argc, argv);
     std::shared_ptr<LocalTransceiver> lcl_trns = std::make_shared<LocalTransceiver>("PLACEHOLDER", SATELLITE_BAUD_RATE);
-    rclcpp::spin(std::make_shared<LocalTransceiverIntf>(lcl_trns));
+    try {
+        std::shared_ptr<LocalTransceiverIntf> node = std::make_shared<LocalTransceiverIntf>(lcl_trns);
+        try {
+            rclcpp::spin(node);
+        } catch (std::exception & e) {
+            RCLCPP_ERROR(node->get_logger(), "%s", e.what());
+            throw e;
+        }
+    } catch (std::exception & e) {
+        std::cerr << e.what() << std::endl;
+        err = true;
+    }
     rclcpp::shutdown();
-    return 0;
+    return err ? -1 : 0;
 }

projects/mock_ais/src/mock_ais_ros_intf.cpp

@@ -12,14 +12,15 @@
 #include "cmn_hdrs/ros_info.h"
 #include "cmn_hdrs/shared_constants.h"
 #include "mock_ais.h"
+#include "net_node.h"
 
 /**
  * Connect the Mock AIS to the onbaord ROS network
  */
-class MockAisRosIntf : public rclcpp::Node
+class MockAisRosIntf : public NetNode
 {
 public:
-    MockAisRosIntf() : Node("mock_ais_node")
+    MockAisRosIntf() : NetNode("mock_ais_node")
     {
         static constexpr int ROS_Q_SIZE = 5;
         this->declare_parameter("enabled", false);
@@ -118,8 +119,20 @@ class MockAisRosIntf : public rclcpp::Node
 
 int main(int argc, char * argv[])
 {
+    bool err = false;
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<MockAisRosIntf>());
+    try {
+        std::shared_ptr<MockAisRosIntf> node = std::make_shared<MockAisRosIntf>();
+        try {
+            rclcpp::spin(node);
+        } catch (std::exception & e) {
+            RCLCPP_ERROR(node->get_logger(), "%s", e.what());
+            throw e;
+        }
+    } catch (std::exception & e) {
+        std::cerr << e.what() << std::endl;
+        err = true;
+    }
     rclcpp::shutdown();
-    return 0;
+    return err ? -1 : 0;
 }

projects/remote_transceiver/src/remote_transceiver_ros_intf.cpp

@@ -7,17 +7,18 @@
 #include <stdexcept>
 
 #include "cmn_hdrs/shared_constants.h"
+#include "net_node.h"
 #include "remote_transceiver.h"
 #include "sailbot_db.h"
 
 /**
  * @brief Connect the Remote Transceiver to the onboard ROS network
  *
  */
-class RemoteTransceiverRosIntf : public rclcpp::Node
+class RemoteTransceiverRosIntf : public NetNode
 {
 public:
-    RemoteTransceiverRosIntf() : Node("remote_transceiver_node")
+    RemoteTransceiverRosIntf() : NetNode("remote_transceiver_node")
     {
         this->declare_parameter("enabled", true);
         enabled_ = this->get_parameter("enabled").as_bool();
@@ -117,14 +118,21 @@ class RemoteTransceiverRosIntf : public rclcpp::Node
 
 int main(int argc, char ** argv)
 {
+    bool err = false;
     rclcpp::init(argc, argv);
     try {
-        rclcpp::spin(std::make_shared<RemoteTransceiverRosIntf>());
-    } catch (std::runtime_error & e) {
+        std::shared_ptr<RemoteTransceiverRosIntf> node = std::make_shared<RemoteTransceiverRosIntf>();
+        try {
+            rclcpp::spin(node);
+        } catch (std::exception & e) {
+            RCLCPP_ERROR(node->get_logger(), "%s", e.what());
+            throw e;
+        }
+    } catch (std::exception & e) {
         std::cerr << e.what() << std::endl;
-        return -1;
+        err = true;
     }
     rclcpp::shutdown();
 
-    return 0;
+    return err ? -1 : 0;
 }