Simplesim log subscribing

softwareComponents/messageLogger/include/message_logger.hpp

@@ -20,7 +20,7 @@ class MessageLogger {
 
     void logSending( std::string_view topic, const google::protobuf::Message & msg )
     {
-        if ( !createFormattedString() ) {
+        if ( !loggingEnabled() ) {
             return;
         }
 
@@ -29,9 +29,10 @@ class MessageLogger {
                           topic,
                           msg.ShortDebugString() ) );
     }
+
     void logReceived( std::string_view topic, const google::protobuf::Message & msg )
     {
-        if ( !createFormattedString() ) {
+        if ( !loggingEnabled() ) {
             return;
         }
 
@@ -41,8 +42,31 @@ class MessageLogger {
                           msg.ShortDebugString() ) );
     }
 
+    void logSubscribe( std::string_view topic )
+    {
+        if ( !loggingEnabled() ) {
+            return;
+        }
+
+        log( fmt::format( "{:%H:%M:%S} Subscribing to '{}':\n",
+                          std::chrono::system_clock::now(),
+                          topic ) );
+    }
+
+    void logUnsubscribe( std::string_view topic )
+    {
+        if ( !loggingEnabled() ) {
+            return;
+        }
+
+        log( fmt::format( "{:%H:%M:%S} Unsubscribing from '{}':\n",
+                          std::chrono::system_clock::now(),
+                          topic ) );
+    }
+
 private:
-    bool createFormattedString()
+    // Prevents unnecessary creation of the log string
+    bool loggingEnabled()
     {
         return _verbose || _ostr;
     }

softwareComponents/rofiHalSim/CMakeLists.txt

@@ -1,8 +1,12 @@
 
 cmake_minimum_required(VERSION 3.11)
 
+set(SRC
+    rofi_hal.cpp
+    session_id.cpp
+)
 
-add_library(rofi_hal_sim SHARED rofi_hal.cpp)
+add_library(rofi_hal_sim SHARED ${SRC})
 target_link_libraries(rofi_hal_sim PRIVATE ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} rofisimMessages atoms)
 target_link_libraries(rofi_hal_sim PUBLIC rofi::hal::inc)
 target_include_directories(rofi_hal_sim SYSTEM PRIVATE ${GAZEBO_INCLUDE_DIRS})

softwareComponents/rofiHalSim/message_logger.hpp

@@ -37,4 +37,19 @@ inline void logMessage( [[maybe_unused]] const std::string & topic,
 #endif
 }
 
+inline void logSubscription( [[maybe_unused]] const std::string & topic,
+                        [[maybe_unused]] bool starting )
+{
+#if LOG_MESSAGES
+    auto now = std::chrono::system_clock::to_time_t( std::chrono::system_clock::now() );
+    if ( starting ) {
+        std::cerr << std::put_time( std::localtime( &now ), "%T" ) << " Subscribing to '" << topic
+                  << std::endl;
+    } else {
+        std::cerr << std::put_time( std::localtime( &now ), "%T" ) << " Unsubscribing from '" << topic
+                  << std::endl;
+    }
+#endif
+}
+
 } // namespace rofi::hal

softwareComponents/rofiHalSim/publish_worker.hpp

@@ -15,6 +15,7 @@
 
 #include "gazebo_node_handler.hpp"
 #include "message_logger.hpp"
+#include "session_id.hpp"
 #include "subscriber_wrapper.hpp"
 
 #include <distributorReq.pb.h>
@@ -110,6 +111,7 @@ class PublishWorker {
 
         rofi::messages::DistributorReq req;
         req.set_reqtype( rofi::messages::DistributorReq::GET_INFO );
+        req.set_sessionid( SessionId::get().bytes() );
         publish( std::move( req ) );
 
         _onRofiTopicsUpdate.wait( lock, [ this, rofiId ] {

softwareComponents/rofiHalSim/rofi_hal.cpp

@@ -13,71 +13,23 @@
 #include <thread>
 #include <utility>
 
-#include <boost/uuid/random_generator.hpp>
-
 #include "connector_worker.hpp"
 #include "joint_worker.hpp"
 #include "publish_worker.hpp"
+#include "session_id.hpp"
 #include "wait_worker.hpp"
 
 #include <distributorReq.pb.h>
 #include <distributorResp.pb.h>
 #include <rofiCmd.pb.h>
 #include <rofiResp.pb.h>
 
-#ifndef SESSION_ID
-#define SESSION_ID boost::uuids::random_generator()()
-#endif
-
 
 namespace
 {
 using namespace rofi::hal;
 namespace msgs = rofi::messages;
 
-class SessionId {
-public:
-    static const SessionId & get()
-    {
-        static SessionId instance = SessionId( SESSION_ID );
-        return instance;
-    }
-
-    const std::string & bytes() const
-    {
-        return _bytes;
-    }
-
-private:
-    template < typename T, std::enable_if_t< std::is_integral_v< T >, int > = 0 >
-    constexpr SessionId( T id )
-    {
-        _bytes.reserve( sizeof( T ) );
-
-        for ( size_t i = 0; i < sizeof( T ); i++ ) {
-            _bytes.push_back( *( reinterpret_cast< char * >( &id ) + i ) );
-        }
-    }
-
-    template < typename T,
-               std::enable_if_t< sizeof( typename T::value_type ) == sizeof( char ), int > = 0 >
-    constexpr SessionId( T id )
-    {
-        _bytes.reserve( id.size() );
-
-        for ( auto & c : id ) {
-            static_assert( sizeof( decltype( c ) ) == sizeof( char ) );
-            _bytes.push_back( reinterpret_cast< char & >( c ) );
-        }
-    }
-
-    SessionId( const SessionId & other ) = delete;
-    SessionId & operator=( const SessionId & other ) = delete;
-
-private:
-    std::string _bytes;
-};
-
 class ConnectorSim;
 class JointSim;
 

softwareComponents/rofiHalSim/session_id.cpp

@@ -0,0 +1,16 @@
+#include "session_id.hpp"
+
+#include <boost/uuid/random_generator.hpp>
+
+
+#ifndef SESSION_ID
+    #define SESSION_ID boost::uuids::random_generator()()
+#endif
+
+using rofi::hal::SessionId;
+
+const SessionId & SessionId::get()
+{
+    static SessionId instance = SessionId( SESSION_ID );
+    return instance;
+}

softwareComponents/rofiHalSim/session_id.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <string>
+#include <type_traits>
+
+
+namespace rofi::hal
+{
+
+class SessionId {
+public:
+    static const SessionId & get();
+
+    const std::string & bytes() const
+    {
+        return _bytes;
+    }
+
+private:
+    template < typename T, std::enable_if_t< std::is_integral_v< T >, int > = 0 >
+    constexpr SessionId( T id )
+    {
+        _bytes.reserve( sizeof( T ) );
+
+        for ( size_t i = 0; i < sizeof( T ); i++ ) {
+            _bytes.push_back( *( reinterpret_cast< char * >( &id ) + i ) );
+        }
+    }
+
+    template < typename T,
+               std::enable_if_t< sizeof( typename T::value_type ) == sizeof( char ), int > = 0 >
+    constexpr SessionId( T id )
+    {
+        _bytes.reserve( id.size() );
+
+        for ( auto & c : id ) {
+            static_assert( sizeof( decltype( c ) ) == sizeof( char ) );
+            _bytes.push_back( reinterpret_cast< char & >( c ) );
+        }
+    }
+
+    SessionId( const SessionId & other ) = delete;
+    SessionId & operator=( const SessionId & other ) = delete;
+
+private:
+    std::string _bytes;
+};
+
+} // namespace rofi::hal

softwareComponents/rofiHalSim/subscriber_wrapper.hpp

@@ -24,6 +24,8 @@ class SubscriberWrapper {
             throw std::runtime_error( "empty callback" );
         }
 
+        logSubscription( _topic, true );
+
         _sub = _node->Subscribe( _topic, &SubscriberWrapper::onMsg, this );
     }
 
@@ -33,6 +35,7 @@ class SubscriberWrapper {
     ~SubscriberWrapper()
     {
         if ( _sub ) {
+            logSubscription( _sub->GetTopic(), false );
             _sub->Unsubscribe();
         }
     }

softwareComponents/simplesimLib/include/simplesim/distributor.hpp

@@ -39,6 +39,7 @@ class Distributor {
     ~Distributor()
     {
         assert( _sub );
+        _logger.logUnsubscribe( _sub->GetTopic() );
         _sub->Unsubscribe();
     }
 
@@ -66,7 +67,7 @@ class Distributor {
         onRequest( *reqCopy );
     }
 
-    rofi::messages::DistributorResp onGetInfoReq();
+    rofi::messages::DistributorResp onGetInfoReq( SessionId sessionId );
     rofi::messages::DistributorResp onLockOneReq( SessionId sessionId );
     rofi::messages::DistributorResp onTryLockReq( ModuleId moduleId, SessionId sessionId );
     rofi::messages::DistributorResp onUnlockReq( ModuleId moduleId, SessionId sessionId );

softwareComponents/simplesimLib/include/simplesim/locked_module_communication.hpp

@@ -33,6 +33,7 @@ class LockedModuleCommunication {
     ~LockedModuleCommunication()
     {
         assert( _sub );
+        _logger.logUnsubscribe( _sub->GetTopic() );
         _sub->Unsubscribe();
     }
 

softwareComponents/simplesimLib/src/distributor.cpp

@@ -14,6 +14,7 @@ Distributor::Distributor( gazebo::transport::Node & node,
     if ( !_pub ) {
         throw std::runtime_error( "Publisher could not be created" );
     }
+    _logger.logSubscribe( "~/distributor/request" );
     _sub = node.Subscribe( "~/distributor/request", &Distributor::onRequestCallback, this );
     if ( !_sub ) {
         throw std::runtime_error( "Subcriber could not be created" );
@@ -34,7 +35,7 @@ void Distributor::onRequest( const rofi::messages::DistributorReq & req )
             if ( req.rofiid() != 0 ) {
                 std::cerr << "Got GET_INFO distributor request with non-zero id\n";
             }
-            sendResponse( onGetInfoReq() );
+            sendResponse( onGetInfoReq( req.sessionid() ) );
             break;
         }
         case DistributorReq::LOCK_ONE:
@@ -64,10 +65,11 @@ void Distributor::onRequest( const rofi::messages::DistributorReq & req )
     }
 }
 
-rofi::messages::DistributorResp Distributor::onGetInfoReq()
+rofi::messages::DistributorResp Distributor::onGetInfoReq( SessionId sessionId )
 {
     rofi::messages::DistributorResp resp;
     resp.set_resptype( rofi::messages::DistributorReq::GET_INFO );
+    resp.set_sessionid( sessionId );
 
     _modulesCommunication.forEachLockedModule(
             [ &resp ]( ModuleId moduleId, const std::string & topic ) {

softwareComponents/simplesimLib/src/locked_module_communication.cpp

@@ -21,6 +21,7 @@ LockedModuleCommunication::LockedModuleCommunication( CommandHandler & commandHa
     assert( !moduleTopicName.empty() );
     assert( _pub );
     assert( _sub );
+    _logger.logSubscribe( "~/" + moduleTopicName + "/control" );
 }
 
 void LockedModuleCommunication::onRofiCmd( const LockedModuleCommunication::RofiCmdPtr & msg )