diff --git a/smacc/include/smacc/client_bases/smacc_subscriber_client.h b/smacc/include/smacc/client_bases/smacc_subscriber_client.h index bcc7cab24..e36b9403a 100644 --- a/smacc/include/smacc/client_bases/smacc_subscriber_client.h +++ b/smacc/include/smacc/client_bases/smacc_subscriber_client.h @@ -10,6 +10,8 @@ #include #include +#include + namespace smacc { @@ -43,6 +45,7 @@ class SmaccSubscriberClient : public smacc::ISmaccClient } smacc::SmaccSignal onFirstMessageReceived_; + smacc::SmaccSignal testSignal; smacc::SmaccSignal onMessageReceived_; std::function 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 + boost::signals2::connection onMessageReceived(void (T::*callback)(const MessageType &), std::shared_ptr object) + { + return this->getStateMachine()->createSignalConnectionNew(onMessageReceived_, std::bind(callback, object.get(), std::placeholders::_1), object); + } + + + template + boost::signals2::connection onMessageReceived(std::function callback, std::shared_ptr object) + { + return this->getStateMachine()->createSignalConnectionNew(onMessageReceived_, callback, object); + } + template boost::signals2::connection onFirstMessageReceived(void (T::*callback)(const MessageType &), T *object) { diff --git a/smacc/include/smacc/impl/smacc_client_behavior_impl.h b/smacc/include/smacc/impl/smacc_client_behavior_impl.h index 499ce4d3c..578bc620a 100644 --- a/smacc/include/smacc/impl/smacc_client_behavior_impl.h +++ b/smacc/include/smacc/impl/smacc_client_behavior_impl.h @@ -56,6 +56,12 @@ void ISmaccClientBehavior::requiresClient(SmaccClientType *&storage) currentOrthogonal->requiresClient(storage); } +template +std::shared_ptr ISmaccClientBehavior::requiresClient() +{ + return currentOrthogonal->requiresClient(); +} + template void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage) { @@ -69,6 +75,19 @@ void ISmaccClientBehavior::requiresComponent(SmaccComponentType *&storage) } } +template +std::shared_ptr 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(); + } +} + template void ISmaccClientBehavior::onOrthogonalAllocation() {} diff --git a/smacc/include/smacc/impl/smacc_client_impl.h b/smacc/include/smacc/impl/smacc_client_impl.h index 07bf14593..5a8cd7591 100644 --- a/smacc/include/smacc/impl/smacc_client_impl.h +++ b/smacc/include/smacc/impl/smacc_client_impl.h @@ -6,6 +6,7 @@ #pragma once +#include #include #include @@ -47,6 +48,30 @@ namespace smacc return nullptr; } + template + std::shared_ptr ISmaccClient::getComponentNew() + { + return this->getComponentNew(std::string()); + } + + template + std::shared_ptr ISmaccClient::getComponentNew(std::string name) + { + for(auto &component : components_) + { + if(component.first.name != name) + continue; + + auto cast_component = std::dynamic_pointer_cast(component.second); + if(cast_component != nullptr) + { + return cast_component; + } + } + + return nullptr; + } + //inline ISmaccStateMachine *ISmaccClient::getStateMachine() { diff --git a/smacc/include/smacc/impl/smacc_orthogonal_impl.h b/smacc/include/smacc/impl/smacc_orthogonal_impl.h index f1a9e86f2..29fe81529 100644 --- a/smacc/include/smacc/impl/smacc_orthogonal_impl.h +++ b/smacc/include/smacc/impl/smacc_orthogonal_impl.h @@ -8,10 +8,44 @@ #include #include #include +#include namespace smacc { +template +std::shared_ptr ISmaccOrthogonal::requiresClient() +{ + for(const auto& client : clients_) + { + const auto cast_client = std::dynamic_pointer_cast(client); + if(cast_client != nullptr) + { + return cast_client; + } + } + + const auto requiredClientName = demangledTypeName(); + 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::value) continue; // ignore current orthogonal + for (const auto& client : orthoentry.second->getClients()) + { + const auto cast_client = std::dynamic_pointer_cast(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 bool ISmaccOrthogonal::requiresClient(SmaccClientType *&storage) { diff --git a/smacc/include/smacc/impl/smacc_state_machine_impl.h b/smacc/include/smacc/impl/smacc_state_machine_impl.h index 37e96831d..a10cde130 100644 --- a/smacc/include/smacc/impl/smacc_state_machine_impl.h +++ b/smacc/include/smacc/impl/smacc_state_machine_impl.h @@ -133,6 +133,30 @@ namespace smacc // storage = ret; } + + template + std::shared_ptr ISmaccStateMachine::requiresComponent() + { + ROS_DEBUG("component %s is required", demangleSymbol(typeid(SmaccComponentType).name()).c_str()); + std::lock_guard lock(m_mutex_); + + for (auto ortho : this->orthogonals_) + { + for (auto &client : ortho.second->clients_) + { + + auto component = client->getComponent(); + 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 void ISmaccStateMachine::postEvent(EventType *ev, EventLifeTime evlifetime) @@ -334,6 +358,53 @@ namespace smacc return connection; } + template + boost::signals2::connection ISmaccStateMachine::createSignalConnectionNew(TSmaccSignal &signal, + TMemberFunctionPrototype callback, + std::shared_ptr object) + { + static_assert(std::is_base_of::value || + std::is_base_of::value || + std::is_base_of::value || + std::is_base_of::value || + std::is_base_of::value, + "Only are accepted smacc types as subscribers for smacc signals"); + + // typedef decltype(callback) ft; + // typedef typename decltype(signal)::SmaccSignalType st; + // Bind::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::value || + std::is_base_of::value || + std::is_base_of::value || + std::is_base_of::value) + { + } + else if (std::is_base_of::value || + std::is_base_of::value || + std::is_base_of::value) + { + ROS_INFO("[StateMachine] life-time constrained smacc signal subscription created. Subscriber is %s", + demangledTypeName().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 // boost::signals2::connection ISmaccStateMachine::createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype // callback) diff --git a/smacc/include/smacc/smacc_client.h b/smacc/include/smacc/smacc_client.h index 45f64650e..8752ba619 100644 --- a/smacc/include/smacc/smacc_client.h +++ b/smacc/include/smacc/smacc_client.h @@ -56,6 +56,12 @@ class ISmaccClient template TComponent *getComponent(std::string name); + template + std::shared_ptr getComponentNew(); + + template + std::shared_ptr getComponentNew(std::string name); + virtual smacc::introspection::TypeInfo::Ptr getType(); inline ISmaccStateMachine *getStateMachine(); diff --git a/smacc/include/smacc/smacc_client_behavior_base.h b/smacc/include/smacc/smacc_client_behavior_base.h index 1cdd646c2..d0f973d9a 100644 --- a/smacc/include/smacc/smacc_client_behavior_base.h +++ b/smacc/include/smacc/smacc_client_behavior_base.h @@ -23,9 +23,15 @@ namespace smacc template void requiresClient(SmaccClientType *&storage); + template + std::shared_ptr requiresClient(); + template void requiresComponent(SmaccComponentType *&storage); + template + std::shared_ptr requiresComponent(); + ros::NodeHandle getNode(); protected: diff --git a/smacc/include/smacc/smacc_orthogonal.h b/smacc/include/smacc/smacc_orthogonal.h index 9d5163299..f7d0b0644 100644 --- a/smacc/include/smacc/smacc_orthogonal.h +++ b/smacc/include/smacc/smacc_orthogonal.h @@ -30,6 +30,9 @@ class ISmaccOrthogonal template void requiresComponent(SmaccComponentType *&storage); + template + std::shared_ptr requiresClient(); + template bool requiresClient(SmaccClientType *&storage); diff --git a/smacc/include/smacc/smacc_signal.h b/smacc/include/smacc/smacc_signal.h index 0298f03ec..883488f15 100644 --- a/smacc/include/smacc/smacc_signal.h +++ b/smacc/include/smacc/smacc_signal.h @@ -24,5 +24,9 @@ template class SmaccSignal : public boost::signals2::signal { + public: + typedef typename boost::signals2::signal SmaccSignalType; + typedef typename SmaccSignalType::slot_type SmaccSlotType; + }; } // namespace smacc diff --git a/smacc/include/smacc/smacc_state_machine.h b/smacc/include/smacc/smacc_state_machine.h index fe04c1535..0dab22787 100644 --- a/smacc/include/smacc/smacc_state_machine.h +++ b/smacc/include/smacc/smacc_state_machine.h @@ -67,6 +67,9 @@ class ISmaccStateMachine template void requiresComponent(SmaccComponentType *&storage); + template + std::shared_ptr requiresComponent(); + template void postEvent(EventType *ev, EventLifeTime evlifetime = EventLifeTime::ABSOLUTE); @@ -100,6 +103,9 @@ class ISmaccStateMachine template boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object); + template + boost::signals2::connection createSignalConnectionNew(TSmaccSignal &signal, TMemberFunctionPrototype callback, std::shared_ptr object); + // template // boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback); diff --git a/smacc_sm_reference_library/sm_ferrari/include/sm_ferrari/clients/cl_subscriber/client_behaviors/cb_my_subscriber_behavior.h b/smacc_sm_reference_library/sm_ferrari/include/sm_ferrari/clients/cl_subscriber/client_behaviors/cb_my_subscriber_behavior.h index 5bf5104b1..558e668bc 100644 --- a/smacc_sm_reference_library/sm_ferrari/include/sm_ferrari/clients/cl_subscriber/client_behaviors/cb_my_subscriber_behavior.h +++ b/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 +#include namespace sm_ferrari { @@ -13,15 +14,26 @@ struct EvMyBehavior: sc::event> }; -class CbMySubscriberBehavior : public smacc::SmaccClientBehavior +class CbMySubscriberBehavior : public smacc::SmaccClientBehavior, public std::enable_shared_from_this { public: void onEntry() { - ClSubscriber* client; - this->requiresClient(client); + std::weak_ptr client = this->requiresClient(); - 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