Adding shared_ptr client behaviour accessors to long-lived client and component objects

smacc/include/smacc/component.h

@@ -40,9 +40,15 @@ class ISmaccComponent
     template <typename TComponent>
     void requiresComponent(TComponent *& requiredComponentStorage);
 
+    template <typename TComponent>
+    void requiresComponent(std::shared_ptr<TComponent>& requiredComponentStorage);
+
     template <typename TClient>
     void requiresClient(TClient *& requiredClientStorage);
 
+    template <typename TClient>
+    void requiresClient(std::shared_ptr<TClient>& requiredClientStorage);
+
     virtual void onInitialize();
 
     template <typename SmaccComponentType, typename TOrthogonal, typename TClient, typename... TArgs>

smacc/include/smacc/impl/smacc_client_behavior_impl.h

@@ -56,6 +56,12 @@ void ISmaccClientBehavior::requiresClient(SmaccClientType *&storage)
     currentOrthogonal->requiresClient(storage);
 }
 
+template <typename SmaccClientType>
+void ISmaccClientBehavior::requiresClient(std::shared_ptr<SmaccClientType>& storage)
+{
+    currentOrthogonal->requiresClient(storage);
+}
+
 template <typename SmaccComponentType>
 void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage)
 {
@@ -69,6 +75,19 @@ void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage)
     }
 }
 
+template <typename SmaccComponentType>
+void ISmaccClientBehavior::requiresComponent(std::shared_ptr<SmaccComponentType>& storage)
+{
+    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
+    {
+        stateMachine_->requiresComponent(storage);
+    }
+}
+
 template <typename TOrthogonal, typename TSourceObject>
 void ISmaccClientBehavior::onOrthogonalAllocation() {}
 

smacc/include/smacc/impl/smacc_client_impl.h

@@ -26,25 +26,50 @@ namespace smacc
     template <typename TComponent>
     TComponent *ISmaccClient::getComponent()
     {
-        return this->getComponent<TComponent>(std::string());
+        std::shared_ptr<TComponent> ptr;
+        this->getComponent<TComponent>(ptr);
+
+        return ptr.get();
+    }
+
+    // template <typename TComponent>
+    // TComponent *ISmaccClient::getComponent(std::string name)
+    // {
+    //     for (auto &component : components_)
+    //     {
+    //         if (component.first.name != name)
+    //             continue;
+
+    //         auto *tcomponent = dynamic_cast<TComponent *>(component.second.get());
+    //         if (tcomponent != nullptr)
+    //         {
+    //             return tcomponent;
+    //         }
+    //     }
+
+    //     return nullptr;
+    // }
+
+    template <typename TComponent>
+    void ISmaccClient::getComponent(std::shared_ptr<TComponent>& storage)
+    {
+        this->getComponent<TComponent>(std::string(), storage);
     }
 
     template <typename TComponent>
-    TComponent *ISmaccClient::getComponent(std::string name)
+    void ISmaccClient::getComponent(std::string name, std::shared_ptr<TComponent>& storage)
     {
         for (auto &component : components_)
         {
             if (component.first.name != name)
                 continue;
 
-            auto *tcomponent = dynamic_cast<TComponent *>(component.second.get());
-            if (tcomponent != nullptr)
+            storage = std::dynamic_pointer_cast<TComponent >(component.second);
+            if (storage != nullptr)
             {
-                return tcomponent;
+                return;
             }
         }
-
-        return nullptr;
     }
 
     //inline
@@ -104,4 +129,10 @@ namespace smacc
         this->orthogonal_->requiresClient(storage);
     }
 
+    template <typename SmaccClientType>
+    void ISmaccClient::requiresClient(std::shared_ptr<SmaccClientType>& storage)
+    {
+        this->orthogonal_->requiresClient(storage);
+    }
+
 } // namespace smacc

smacc/include/smacc/impl/smacc_component_impl.h

@@ -22,13 +22,24 @@ namespace smacc
     {
         requiredComponentStorage = this->owner_->getComponent<TComponent>();
     }
+    template <typename TComponent>
+    void ISmaccComponent::requiresComponent(std::shared_ptr<TComponent> &requiredComponentStorage)
+    {
+        requiredComponentStorage = this->owner_->getComponent<TComponent>();
+    }
 
     template <typename TClient>
     void ISmaccComponent::requiresClient(TClient *&requiredClientStorage)
     {
         this->owner_->requiresClient(requiredClientStorage);
     }
 
+    template <typename TClient>
+    void ISmaccComponent::requiresClient(std::shared_ptr<TClient> &requiredClientStorage)
+    {
+        this->owner_->requiresClient(requiredClientStorage);
+    }
+
     template <typename SmaccComponentType, typename TOrthogonal, typename TClient, typename... TArgs>
     SmaccComponentType *ISmaccComponent::createSiblingComponent(TArgs... targs)
     {

smacc/include/smacc/impl/smacc_orthogonal_impl.h

@@ -42,6 +42,36 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *&storage)
     return false;
 }
 
+template <typename SmaccClientType>
+bool ISmaccOrthogonal::requiresClient(std::shared_ptr<SmaccClientType> &storage)
+{
+    for (auto &client : clients_)
+    {
+        storage = std::dynamic_pointer_cast<SmaccClientType>(client);
+        if (storage != nullptr)
+            return true;
+    }
+
+    auto requiredClientName = demangledTypeName<SmaccClientType>();
+    ROS_WARN_STREAM("Required client ["<< requiredClientName<< "] not found in current orthogonal. Searching in other orthogonals.");
+
+    for (auto &orthoentry : this->getStateMachine()->getOrthogonals())
+    {
+        for (auto &client : orthoentry.second->getClients())
+        {
+            storage = std::dynamic_pointer_cast<SmaccClientType>(client);
+            if (storage != nullptr)
+            {
+                ROS_WARN_STREAM("Required client  ["<< requiredClientName<<"] found in other orthogonal.");
+                return true;
+            }
+        }
+    }
+
+    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 false;
+}
+
 template <typename SmaccComponentType>
 void ISmaccOrthogonal::requiresComponent(SmaccComponentType *&storage)
 {
@@ -55,6 +85,19 @@ void ISmaccOrthogonal::requiresComponent(SmaccComponentType *&storage)
     }
 }
 
+template <typename SmaccComponentType>
+void ISmaccOrthogonal::requiresComponent(std::shared_ptr<SmaccComponentType> &storage)
+{
+    if (stateMachine_ == nullptr)
+    {
+        ROS_ERROR("Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
+    }
+    else
+    {
+        stateMachine_->requiresComponent(storage);
+    }
+}
+
 template <typename TOrthogonal, typename TClient>
 void ISmaccOrthogonal::assignClientToOrthogonal(TClient* client)
 {

smacc/include/smacc/impl/smacc_state_impl.h

@@ -72,6 +72,12 @@ namespace smacc
     {
         this->getStateMachine().requiresComponent(storage);
     }
+
+    template <typename SmaccComponentType>
+    void ISmaccState::requiresComponent(std::shared_ptr<SmaccComponentType> &storage)
+    {
+        this->getStateMachine().requiresComponent(storage);
+    }
     //-------------------------------------------------------------------------------------------------------
 
     template <typename SmaccClientType>
@@ -89,6 +95,22 @@ namespace smacc
 
         ROS_ERROR("[%s] Client of type '%s' not found in any orthogonal of the current state machine. This may produce a segmentation fault if the returned reference is used.", sname, demangleSymbol<SmaccClientType>().c_str());
     }
+
+    template <typename SmaccClientType>
+    void ISmaccState::requiresClient(std::shared_ptr<SmaccClientType>& storage)
+    {
+        const char *sname = (demangleSymbol(typeid(*this).name()).c_str());
+        storage = nullptr;
+        auto &orthogonals = this->getStateMachine().getOrthogonals();
+        for (auto &ortho : orthogonals)
+        {
+            ortho.second->requiresClient(storage);
+            if (storage != nullptr)
+                return;
+        }
+
+        ROS_ERROR("[%s] Client of type '%s' not found in any orthogonal of the current state machine. This may produce a segmentation fault if the returned reference is used.", sname, demangleSymbol<SmaccClientType>().c_str());
+    }
     //-------------------------------------------------------------------------------------------------------
 
     template <typename T>

smacc/include/smacc/impl/smacc_state_machine_impl.h

@@ -133,6 +133,50 @@ namespace smacc
 
     // storage = ret;
   }
+
+  template <typename SmaccComponentType>
+  void ISmaccStateMachine::requiresComponent(std::shared_ptr<SmaccComponentType> &storage)
+  {
+    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_)
+      {
+
+        storage = client->getComponent<SmaccComponentType>();
+        if (storage != nullptr)
+        {
+          return;
+        }
+      }
+    }
+
+    ROS_WARN("component %s is required but it was not found in any orthogonal", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
+
+    // std::string componentkey = demangledTypeName<SmaccComponentType>();
+    // SmaccComponentType *ret;
+
+    // auto it = components_.find(componentkey);
+
+    // if (it == components_.end())
+    // {
+    //     ROS_DEBUG("%s smacc component is required. Creating a new instance.", componentkey.c_str());
+
+    //     ret = new SmaccComponentType();
+    //     ret->setStateMachine(this);
+    //     components_[componentkey] = static_cast<smacc::ISmaccComponent *>(ret);
+    //     ROS_DEBUG("%s resource is required. Done.", componentkey.c_str());
+    // }
+    // else
+    // {
+    //     ROS_DEBUG("%s resource is required. Found resource in cache.", componentkey.c_str());
+    //     ret = dynamic_cast<SmaccComponentType *>(it->second);
+    // }
+
+    // storage = ret;
+  }
   //-------------------------------------------------------------------------------------------------------
   template <typename EventType>
   void ISmaccStateMachine::postEvent(EventType *ev, EventLifeTime evlifetime)

smacc/include/smacc/smacc_client.h

@@ -53,8 +53,14 @@ class ISmaccClient
     template <typename TComponent>
     TComponent *getComponent();
 
+    // template <typename TComponent>
+    // TComponent *getComponent(std::string name);
+
+    template <typename TComponent>
+    void getComponent(std::shared_ptr<TComponent>& storage);
+
     template <typename TComponent>
-    TComponent *getComponent(std::string name);
+    void getComponent(std::string name, std::shared_ptr<TComponent>& storage );
 
     virtual smacc::introspection::TypeInfo::Ptr getType();
 
@@ -66,6 +72,9 @@ class ISmaccClient
     template <typename SmaccClientType>
     void requiresClient(SmaccClientType *&storage);
 
+    template <typename SmaccClientType>
+    void requiresClient(std::shared_ptr<SmaccClientType>& storage);
+
     void getComponents(std::vector<std::shared_ptr<ISmaccComponent>> &components);
 
 protected:

smacc/include/smacc/smacc_client_behavior_base.h

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

smacc/include/smacc/smacc_orthogonal.h

@@ -30,9 +30,15 @@ class ISmaccOrthogonal
     template <typename SmaccComponentType>
     void requiresComponent(SmaccComponentType *&storage);
 
+    template <typename SmaccComponentType>
+    void requiresComponent(std::shared_ptr<SmaccComponentType> &storage);
+
     template <typename SmaccClientType>
     bool requiresClient(SmaccClientType *&storage);
 
+    template <typename SmaccClientType>
+    bool requiresClient(std::shared_ptr<SmaccClientType>& storage);
+
     inline const std::vector<std::shared_ptr<smacc::ISmaccClient>> &getClients();
 
     inline const std::vector<std::shared_ptr<smacc::ISmaccClientBehavior>> &getClientBehaviors() const;

smacc/include/smacc/smacc_state.h

@@ -25,9 +25,15 @@ namespace smacc
     template <typename SmaccComponentType>
     void requiresComponent(SmaccComponentType *&storage);
 
+    template <typename SmaccComponentType>
+    void requiresComponent(std::shared_ptr<SmaccComponentType> &storage);
+
     template <typename SmaccClientType>
     void requiresClient(SmaccClientType *&storage);
 
+    template <typename SmaccClientType>
+    void requiresClient(std::shared_ptr<SmaccClientType>& storage);
+
     template <typename T>
     bool getGlobalSMData(std::string name, T &ret);
 

smacc/include/smacc/smacc_state_machine.h

@@ -67,6 +67,9 @@ class ISmaccStateMachine
     template <typename SmaccComponentType>
     void requiresComponent(SmaccComponentType *&storage);
 
+    template <typename SmaccComponentType>
+    void requiresComponent(std::shared_ptr<SmaccComponentType> &storage);
+
     template <typename EventType>
     void postEvent(EventType *ev, EventLifeTime evlifetime = EventLifeTime::ABSOLUTE);