Implement smart pointers and signals2 tracking

smacc/include/smacc/client_bases/smacc_subscriber_client.h

@@ -10,6 +10,8 @@
 #include <boost/optional/optional_io.hpp>
 #include <smacc/impl/smacc_state_impl.h>
 
+#include <memory>
+
 namespace smacc
 {
 
@@ -43,6 +45,7 @@ class SmaccSubscriberClient : public smacc::ISmaccClient
   }
 
   smacc::SmaccSignal<void(const MessageType &)> onFirstMessageReceived_;
+  smacc::SmaccSignal<void()> testSignal;
   smacc::SmaccSignal<void(const MessageType &)> onMessageReceived_;
 
   std::function<void(const MessageType &)> postMessageEvent;
@@ -54,6 +57,20 @@ class SmaccSubscriberClient : public smacc::ISmaccClient
     return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object);
   }
 
+  // TODO: Why does the compiler moan when we only use T? They're the same type T==F
+  template <typename T, typename F>
+  boost::signals2::connection onMessageReceived(void (T::*callback)(const MessageType &), std::shared_ptr<F> object)
+  {
+    return this->getStateMachine()->createSignalConnectionNew(onMessageReceived_, std::bind(callback, object.get(), std::placeholders::_1), object);
+  }
+
+
+  template <typename T>
+  boost::signals2::connection onMessageReceived(std::function<void(const MessageType&)> callback, std::shared_ptr<T> object)
+  {
+    return this->getStateMachine()->createSignalConnectionNew(onMessageReceived_, callback, object);
+  }
+
   template <typename T>
   boost::signals2::connection onFirstMessageReceived(void (T::*callback)(const MessageType &), T *object)
   {

smacc/include/smacc/impl/smacc_client_behavior_impl.h

@@ -56,6 +56,12 @@ void ISmaccClientBehavior::requiresClient(SmaccClientType *&storage)
     currentOrthogonal->requiresClient(storage);
 }
 
+template <typename SmaccClientType>
+std::shared_ptr<SmaccClientType> ISmaccClientBehavior::requiresClient()
+{
+    return currentOrthogonal->requiresClient<SmaccClientType>();
+}
+
 template <typename SmaccComponentType>
 void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage)
 {
@@ -69,6 +75,19 @@ void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage)
     }
 }
 
+template <typename SmaccComponentType>
+std::shared_ptr<SmaccComponentType> ISmaccClientBehavior::requiresComponent()
+{
+    if (stateMachine_ == nullptr)
+    {
+        ROS_ERROR("Cannot use the requiresComponent funcionality before assigning the client behavior to an orthogonal. Try using the OnEntry method to capture required components.");
+    }
+    else
+    {
+        return stateMachine_->requiresComponent<SmaccComponentType>();
+    }
+}
+
 template <typename TOrthogonal, typename TSourceObject>
 void ISmaccClientBehavior::onOrthogonalAllocation() {}
 

smacc/include/smacc/impl/smacc_client_impl.h

@@ -6,6 +6,7 @@
 
 #pragma once
 
+#include <memory>
 #include <smacc/smacc_client.h>
 #include <smacc/impl/smacc_state_machine_impl.h>
 
@@ -47,6 +48,30 @@ namespace smacc
         return nullptr;
     }
 
+    template <typename TComponent>
+    std::shared_ptr<TComponent> ISmaccClient::getComponentNew() 
+    {
+        return this->getComponentNew<TComponent>(std::string());
+    }
+
+    template <typename TComponent>
+    std::shared_ptr<TComponent> ISmaccClient::getComponentNew(std::string name)
+    {
+        for(auto &component : components_)
+        {
+            if(component.first.name != name)
+              continue;
+
+            auto cast_component = std::dynamic_pointer_cast<TComponent>(component.second);
+            if(cast_component != nullptr)
+            {
+                return cast_component;
+            }
+        }
+
+        return nullptr;
+    }
+
     //inline
     ISmaccStateMachine *ISmaccClient::getStateMachine()
     {

smacc/include/smacc/impl/smacc_orthogonal_impl.h

@@ -8,10 +8,44 @@
 #include <smacc/smacc_orthogonal.h>
 #include <smacc/smacc_client.h>
 #include <cassert>
+#include <type_traits>
 
 namespace smacc
 {
 
+template <typename SmaccClientType>
+std::shared_ptr<SmaccClientType> ISmaccOrthogonal::requiresClient()
+{
+    for(const auto& client : clients_) 
+    {
+        const auto cast_client = std::dynamic_pointer_cast<SmaccClientType>(client);
+        if(cast_client != nullptr)
+        {
+            return cast_client;
+        }
+    }
+
+    const auto requiredClientName = demangledTypeName<SmaccClientType>();
+    ROS_WARN_STREAM("Required client ["<< requiredClientName<< "] not found in current orthogonal. Searching in other orthogonals.");
+
+    for (const auto &orthoentry : this->getStateMachine()->getOrthogonals())
+    {
+        if(std::is_same<decltype(*this), decltype(orthoentry)>::value) continue;    // ignore current orthogonal
+        for (const auto& client : orthoentry.second->getClients())
+        {
+            const auto cast_client = std::dynamic_pointer_cast<SmaccClientType>(client);
+            if(cast_client != nullptr)
+            {
+                ROS_WARN_STREAM("Required client  ["<< requiredClientName<<"] found in other orthogonal.");
+                return cast_client;
+            }
+        }
+    }
+
+    ROS_ERROR_STREAM("Required client ["<< requiredClientName<< "] not found even in other orthogonals. Returning null pointer. If the requested client is used may result in a segmentation fault.");
+    return nullptr;
+}
+
 template <typename SmaccClientType>
 bool ISmaccOrthogonal::requiresClient(SmaccClientType *&storage)
 {

smacc/include/smacc/impl/smacc_state_machine_impl.h

@@ -133,6 +133,30 @@ namespace smacc
 
     // storage = ret;
   }
+
+  template <typename SmaccComponentType>
+  std::shared_ptr<SmaccComponentType> ISmaccStateMachine::requiresComponent()
+  {
+    ROS_DEBUG("component %s is required", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
+    std::lock_guard<std::recursive_mutex> lock(m_mutex_);
+
+    for (auto ortho : this->orthogonals_)
+    {
+      for (auto &client : ortho.second->clients_)
+      {
+
+        auto component = client->getComponent<SmaccComponentType>();
+        if (component != nullptr)
+        {
+          return component;
+        }
+      }
+    }
+
+    ROS_WARN("component %s is required but it was not found in any orthogonal", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
+
+    return nullptr;
+  }
   //-------------------------------------------------------------------------------------------------------
   template <typename EventType>
   void ISmaccStateMachine::postEvent(EventType *ev, EventLifeTime evlifetime)
@@ -334,6 +358,53 @@ namespace smacc
     return connection;
   }
 
+  template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
+  boost::signals2::connection ISmaccStateMachine::createSignalConnectionNew(TSmaccSignal &signal,
+                                                                         TMemberFunctionPrototype callback,
+                                                                         std::shared_ptr<TSmaccObjectType> object)
+  {
+    static_assert(std::is_base_of<ISmaccState, TSmaccObjectType>::value ||
+                      std::is_base_of<ISmaccClient, TSmaccObjectType>::value ||
+                      std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value ||
+                      std::is_base_of<StateReactor, TSmaccObjectType>::value ||
+                      std::is_base_of<ISmaccComponent, TSmaccObjectType>::value,
+                  "Only are accepted smacc types as subscribers for smacc signals");
+
+    // typedef decltype(callback) ft;
+    // typedef typename decltype(signal)::SmaccSignalType st;
+    // Bind<boost::function_types::function_arity<ft>::value> binder;
+    // auto test = decltype(signal)::SmaccSignalSlotType(callback, object.get(), _1).track(object);
+    // auto test = std::bind(callback, object.get(), _1);
+    // boost::signals2::connection connection = signal.connect(decltype(signal)::SmaccSlotType(callback, object.get(), _1).track(object));
+    // boost::signals2::connection connection = signal.connect(typename TSmaccSignal::SmaccSlotType(callback, object.get(), _1).track(object));
+    boost::signals2::connection connection = signal.connect(typename TSmaccSignal::SmaccSlotType(callback).track_foreign(object));
+    // boost::signals2::connection connection = signal.connect(std::bind(callback, object.get(), _1).track(object));
+    // boost::signals2::connection connection = signal.connect(test.track(object));
+
+    // long life-time objects
+    if (std::is_base_of<ISmaccComponent, TSmaccObjectType>::value ||
+        std::is_base_of<ISmaccClient, TSmaccObjectType>::value ||
+        std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
+        std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
+    {
+    }
+    else if (std::is_base_of<ISmaccState, TSmaccObjectType>::value ||
+             std::is_base_of<StateReactor, TSmaccObjectType>::value ||
+             std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
+    {
+      ROS_INFO("[StateMachine] life-time constrained smacc signal subscription created. Subscriber is %s",
+               demangledTypeName<TSmaccObjectType>().c_str());
+      // stateCallbackConnections.push_back(connection);
+    }
+    else // state life-time objects
+    {
+      ROS_WARN("[StateMachine] connecting signal to an unknown object with life-time unknown behavior. It might provoke "
+               "an exception if the object is destroyed during the execution.");
+    }
+
+    return connection;
+  }
+
   // template <typename TSmaccSignal, typename TMemberFunctionPrototype>
   // boost::signals2::connection ISmaccStateMachine::createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype
   // callback)

smacc/include/smacc/smacc_client.h

@@ -56,6 +56,12 @@ class ISmaccClient
     template <typename TComponent>
     TComponent *getComponent(std::string name);
 
+    template <typename TComponent>
+    std::shared_ptr<TComponent> getComponentNew();
+
+    template <typename TComponent>
+    std::shared_ptr<TComponent> getComponentNew(std::string name);
+
     virtual smacc::introspection::TypeInfo::Ptr getType();
 
     inline ISmaccStateMachine *getStateMachine();

smacc/include/smacc/smacc_client_behavior_base.h

@@ -23,9 +23,15 @@ namespace smacc
         template <typename SmaccClientType>
         void requiresClient(SmaccClientType *&storage);
 
+        template <typename SmaccClientType>
+        std::shared_ptr<SmaccClientType> requiresClient();
+
         template <typename SmaccComponentType>
         void requiresComponent(SmaccComponentType *&storage);
 
+        template <typename SmaccComponentType>
+        std::shared_ptr<SmaccComponentType> requiresComponent();
+
         ros::NodeHandle getNode();
 
     protected:

smacc/include/smacc/smacc_orthogonal.h

@@ -30,6 +30,9 @@ class ISmaccOrthogonal
     template <typename SmaccComponentType>
     void requiresComponent(SmaccComponentType *&storage);
 
+    template <typename SmaccClientType>
+    std::shared_ptr<SmaccClientType> requiresClient();
+
     template <typename SmaccClientType>
     bool requiresClient(SmaccClientType *&storage);
 

smacc/include/smacc/smacc_signal.h

@@ -24,5 +24,9 @@ template <typename Signature,
           typename Mutex = boost::signals2::mutex>
 class SmaccSignal : public boost::signals2::signal<Signature, Combiner, Group, GroupCompare, SlotFunction, ExtendedSlotFunction, Mutex>
 {
+  public:
+    typedef typename boost::signals2::signal<Signature, Combiner, Group, GroupCompare, SlotFunction, ExtendedSlotFunction, Mutex> SmaccSignalType;
+    typedef typename SmaccSignalType::slot_type SmaccSlotType;
+
 };
 } // namespace smacc

smacc/include/smacc/smacc_state_machine.h

@@ -67,6 +67,9 @@ class ISmaccStateMachine
     template <typename SmaccComponentType>
     void requiresComponent(SmaccComponentType *&storage);
 
+    template <typename SmaccComponentType>
+    std::shared_ptr<SmaccComponentType> requiresComponent();
+
     template <typename EventType>
     void postEvent(EventType *ev, EventLifeTime evlifetime = EventLifeTime::ABSOLUTE);
 
@@ -100,6 +103,9 @@ class ISmaccStateMachine
     template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
     boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object);
 
+    template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
+    boost::signals2::connection createSignalConnectionNew(TSmaccSignal &signal, TMemberFunctionPrototype callback, std::shared_ptr<TSmaccObjectType> object);
+
     // template <typename TSmaccSignal, typename TMemberFunctionPrototype>
     // boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback);
 

smacc_sm_reference_library/sm_ferrari/include/sm_ferrari/clients/cl_subscriber/client_behaviors/cb_my_subscriber_behavior.h

@@ -1,6 +1,7 @@
 
 #pragma once
 #include <sm_ferrari/clients/cl_subscriber/cl_subscriber.h>
+#include <memory>
 
 namespace sm_ferrari
 {
@@ -13,15 +14,26 @@ struct EvMyBehavior: sc::event<EvMyBehavior<TSource, TOrthogonal>>
 
 };
 
-class CbMySubscriberBehavior : public smacc::SmaccClientBehavior
+class CbMySubscriberBehavior : public smacc::SmaccClientBehavior, public std::enable_shared_from_this<CbMySubscriberBehavior>
 {
 public:
     void onEntry()
     {
-        ClSubscriber* client;
-        this->requiresClient(client);
+        std::weak_ptr<ClSubscriber> client = this->requiresClient<ClSubscriber>();
 
-        client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, this);
+        if(auto locked_client = client.lock())
+        {
+            // locked_client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, this);
+            //locked_client->onMessageReceived([&](const std_msgs::Float32& msg) -> void 
+            //{
+            //    if(msg.data > 30)
+            //    {
+            //        ROS_INFO("[CbMySubscriberBehavior] received message from topic with value > 30. Posting event!");
+            //        this->postMyEvent_();
+            //    }
+            //}, shared_from_this());
+            locked_client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, shared_from_this());
+        }
     }
 
     template <typename TOrthogonal, typename TSourceObject>