Added RCL logging for BroadcastServer (open-rmf#458)

* Added new API to pass in a logger

Signed-off-by: Saurabh Kamat <kamatsaurabh01@gmail.com>

* Changed test to use new BraodcastServer API

Signed-off-by: Saurabh Kamat <kamatsaurabh01@gmail.com>

* Removed default value of node from logging related ws api

Signed-off-by: Saurabh Kamat <kamatsaurabh01@gmail.com>

* Lint fixes

Signed-off-by: Saurabh Kamat <kamatsaurabh01@gmail.com>

---------

Signed-off-by: Saurabh Kamat <kamatsaurabh01@gmail.com>

Author: sauk2

rmf_fleet_adapter/test/tasks/test_Delivery.cpp

@@ -372,7 +372,7 @@ SCENARIO("Test Delivery")
   /// from the websocket connection
   /* *INDENT-OFF* */
   using WebsocketServer = rmf_websocket::BroadcastServer;
-	const auto ws_server = WebsocketServer::make(
+	const auto ws_server = WebsocketServer::make_with_logger(
 		37878,
     [ &cb_mutex, delivery_id, &completed_promise,
       &at_least_one_incomplete, &fulfilled_promise](
@@ -397,6 +397,7 @@ SCENARIO("Test Delivery")
         else
           at_least_one_incomplete = true;
       },
+    adapter.node()->get_node_logging_interface(),
     WebsocketServer::ApiMsgType::TaskStateUpdate
   );
   /* *INDENT-ON* */

rmf_websocket/include/rmf_websocket/BroadcastServer.hpp

@@ -23,6 +23,8 @@
 #include <optional>
 #include <rmf_utils/impl_ptr.hpp>
 
+#include <rclcpp/rclcpp.hpp>
+
 namespace rmf_websocket {
 
 //==============================================================================
@@ -43,7 +45,8 @@ class BroadcastServer
 
   using ApiMessageCallback = std::function<void(const nlohmann::json&)>;
 
-  /// Add a callback to convert from a Description into an active Task.
+  /// Create a wrapper around a websocket server for receiving states and logs for
+  /// fleets, robots and tasks
   ///
   /// \param[in] port
   ///   server url port number
@@ -59,6 +62,29 @@ class BroadcastServer
     ApiMessageCallback callback,
     std::optional<ApiMsgType> msg_selection = std::nullopt);
 
+  /// Create a wrapper around a websocket server for receiving states and logs for
+  /// fleets, robots and tasks
+  ///
+  /// \param[in] port
+  ///   server url port number
+  ///
+  /// \param[in] callback
+  ///   callback function when the message is received
+  ///
+  /// \param[in] node_logging_interface
+  ///   node interface for logging. Default as nullptr which will result
+  ///   in errors and additional debug information not being logged
+  ///
+  /// \param[in] msg_selection
+  ///   selected msg type to listen. Will listen to all msg if nullopt
+  ///
+  static std::shared_ptr<BroadcastServer> make_with_logger(
+    const int port,
+    ApiMessageCallback callback,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+      node_logging_interface,
+    std::optional<ApiMsgType> msg_selection = std::nullopt);
+
   /// Start Server
   void start();
 

rmf_websocket/src/rmf_websocket/BroadcastServer.cpp

@@ -39,10 +39,20 @@ class BroadcastServer::Implementation
   Implementation(
     const int port,
     ApiMessageCallback callback,
-    std::optional<ApiMsgType> msg_selection)
+    std::optional<ApiMsgType> msg_selection,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+      node_logging_interface)
   : _data(std::make_shared<Data>(std::move(callback),
-      std::move(msg_selection)))
+        std::move(msg_selection))),
+    _logger_interface(node_logging_interface)
   {
+    if (_logger_interface)
+    {
+      RCLCPP_INFO_STREAM(
+        _logger_interface->get_logger(),
+        "Running websocket server on port " << port);
+    }
+
     try
     {
       // Hide all logs from websocketpp
@@ -66,17 +76,42 @@ class BroadcastServer::Implementation
     }
     catch (const websocketpp::exception& e)
     {
-      std::cout << e.what() << std::endl;
+      if (_logger_interface)
+      {
+        RCLCPP_ERROR_STREAM(
+          _logger_interface->get_logger(),
+          "WebSocket exception occured: " << e.what());
+      }
     }
     catch (...)
     {
-      std::cout << "other exception" << std::endl;
+      if (_logger_interface)
+      {
+        RCLCPP_ERROR(
+          _logger_interface->get_logger(), "Unknown exception occured");
+      }
     }
   }
 
   /// Start Server
   void start()
   {
+    if (_logger_interface)
+    {
+      websocketpp::lib::asio::error_code ec;
+      auto endpoint = _data->echo_server.get_local_endpoint(ec);
+      if (ec)
+      {
+        RCLCPP_ERROR_STREAM(
+          _logger_interface->get_logger(),
+          "Failed to create connection: " << ec.message());
+         return;
+      }
+      RCLCPP_INFO_STREAM(
+        _logger_interface->get_logger(),
+        "Starting BroadcastServer on port " << endpoint.port());
+    }
+
     // Start the ASIO io_service run loop
     _server_thread = std::thread(
       [data = _data]() { data->echo_server.run(); });
@@ -87,6 +122,12 @@ class BroadcastServer::Implementation
   {
     if (_server_thread.joinable())
     {
+      if (_logger_interface)
+      {
+        RCLCPP_INFO(
+          _logger_interface->get_logger(), "Stopping BroadcastServer");
+      }
+
       _data->echo_server.get_io_service().post(
           [data = _data]()
           {
@@ -154,6 +195,7 @@ class BroadcastServer::Implementation
   };
   std::shared_ptr<Data> _data;
   std::thread _server_thread;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr _logger_interface;
 };
 
 //==============================================================================
@@ -165,7 +207,22 @@ std::shared_ptr<BroadcastServer> BroadcastServer::make(
   auto server = std::shared_ptr<BroadcastServer>(new BroadcastServer());
   server->_pimpl =
     rmf_utils::make_unique_impl<Implementation>(
-    port, callback, msg_selection);
+    port, callback, msg_selection, nullptr);
+  return server;
+}
+
+//==============================================================================
+std::shared_ptr<BroadcastServer> BroadcastServer::make_with_logger(
+  const int port,
+  ApiMessageCallback callback,
+  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+    node_logging_interface,
+  std::optional<ApiMsgType> msg_selection)
+{
+  auto server = std::shared_ptr<BroadcastServer>(new BroadcastServer());
+  server->_pimpl =
+    rmf_utils::make_unique_impl<Implementation>(
+        port, callback, msg_selection, node_logging_interface);
   return server;
 }