From 84fda7878fbbf6dd54bd714f017876726d42eac6 Mon Sep 17 00:00:00 2001 From: Jacobus Lock Date: Sun, 12 Mar 2023 09:48:45 +0000 Subject: [PATCH 1/4] Added new requiresClient() call that returns the shared_ptr instead of a raw ptr, implemented it in an example to verify it works as expected --- .../smacc/impl/smacc_client_behavior_impl.h | 6 ++++ .../smacc/impl/smacc_orthogonal_impl.h | 34 +++++++++++++++++++ .../smacc/smacc_client_behavior_base.h | 3 ++ smacc/include/smacc/smacc_orthogonal.h | 3 ++ .../cb_my_subscriber_behavior.h | 10 ++++-- 5 files changed, 53 insertions(+), 3 deletions(-) diff --git a/smacc/include/smacc/impl/smacc_client_behavior_impl.h b/smacc/include/smacc/impl/smacc_client_behavior_impl.h index 499ce4d3c..9123468ff 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) { 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/smacc_client_behavior_base.h b/smacc/include/smacc/smacc_client_behavior_base.h index 1cdd646c2..794023ef1 100644 --- a/smacc/include/smacc/smacc_client_behavior_base.h +++ b/smacc/include/smacc/smacc_client_behavior_base.h @@ -23,6 +23,9 @@ namespace smacc template void requiresClient(SmaccClientType *&storage); + template + std::shared_ptr requiresClient(); + template void requiresComponent(SmaccComponentType *&storage); 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_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..3c67de63a 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 @@ -18,10 +18,14 @@ class CbMySubscriberBehavior : public smacc::SmaccClientBehavior public: void onEntry() { - ClSubscriber* client; - this->requiresClient(client); + ROS_ERROR("ALLO"); + std::weak_ptr client = this->requiresClient(); - client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, this); + if(auto locked_client = client.lock()) + { + ROS_ERROR("LOCKED"); + locked_client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, this); + } } template From c6bb57ce9ad881149d2b631087c0a999a701e4ee Mon Sep 17 00:00:00 2001 From: Jacobus Lock Date: Thu, 23 Mar 2023 16:26:19 +0000 Subject: [PATCH 2/4] Added weak_ptr interface for components --- .../smacc/impl/smacc_client_behavior_impl.h | 13 ++++++++++ smacc/include/smacc/impl/smacc_client_impl.h | 25 +++++++++++++++++++ .../smacc/impl/smacc_state_machine_impl.h | 24 ++++++++++++++++++ smacc/include/smacc/smacc_client.h | 6 +++++ .../smacc/smacc_client_behavior_base.h | 3 +++ smacc/include/smacc/smacc_state_machine.h | 3 +++ 6 files changed, 74 insertions(+) diff --git a/smacc/include/smacc/impl/smacc_client_behavior_impl.h b/smacc/include/smacc/impl/smacc_client_behavior_impl.h index 9123468ff..578bc620a 100644 --- a/smacc/include/smacc/impl/smacc_client_behavior_impl.h +++ b/smacc/include/smacc/impl/smacc_client_behavior_impl.h @@ -75,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_state_machine_impl.h b/smacc/include/smacc/impl/smacc_state_machine_impl.h index 37e96831d..b5750db2e 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) 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 794023ef1..d0f973d9a 100644 --- a/smacc/include/smacc/smacc_client_behavior_base.h +++ b/smacc/include/smacc/smacc_client_behavior_base.h @@ -29,6 +29,9 @@ namespace smacc template void requiresComponent(SmaccComponentType *&storage); + template + std::shared_ptr requiresComponent(); + ros::NodeHandle getNode(); protected: diff --git a/smacc/include/smacc/smacc_state_machine.h b/smacc/include/smacc/smacc_state_machine.h index fe04c1535..564350950 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); From 5616643a6cb5f20ab3234ae575a127a05420e698 Mon Sep 17 00:00:00 2001 From: Jacobus Lock Date: Thu, 23 Mar 2023 17:52:10 +0000 Subject: [PATCH 3/4] Added ability to create trackable boost::signal --- .../smacc/impl/smacc_state_machine_impl.h | 40 +++++++++++++++++++ smacc/include/smacc/smacc_state_machine.h | 3 ++ 2 files changed, 43 insertions(+) diff --git a/smacc/include/smacc/impl/smacc_state_machine_impl.h b/smacc/include/smacc/impl/smacc_state_machine_impl.h index b5750db2e..e694c4217 100644 --- a/smacc/include/smacc/impl/smacc_state_machine_impl.h +++ b/smacc/include/smacc/impl/smacc_state_machine_impl.h @@ -358,6 +358,46 @@ 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; + Bind::value> binder; + boost::signals2::connection connection = binder.bindaux(signal, callback, object.get()).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_state_machine.h b/smacc/include/smacc/smacc_state_machine.h index 564350950..0dab22787 100644 --- a/smacc/include/smacc/smacc_state_machine.h +++ b/smacc/include/smacc/smacc_state_machine.h @@ -103,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); From 4b702e94a96d397b46cdefa7744c6a1dab9a0373 Mon Sep 17 00:00:00 2001 From: Jacobus Lock Date: Mon, 27 Mar 2023 15:23:48 +0100 Subject: [PATCH 4/4] Modified signal registrar to use signals2's tracking mechanism --- .../client_bases/smacc_subscriber_client.h | 17 +++++++++++++++++ .../smacc/impl/smacc_state_machine_impl.h | 13 ++++++++++--- smacc/include/smacc/smacc_signal.h | 4 ++++ .../cb_my_subscriber_behavior.h | 16 ++++++++++++---- 4 files changed, 43 insertions(+), 7 deletions(-) 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_state_machine_impl.h b/smacc/include/smacc/impl/smacc_state_machine_impl.h index e694c4217..a10cde130 100644 --- a/smacc/include/smacc/impl/smacc_state_machine_impl.h +++ b/smacc/include/smacc/impl/smacc_state_machine_impl.h @@ -370,9 +370,16 @@ namespace smacc std::is_base_of::value, "Only are accepted smacc types as subscribers for smacc signals"); - typedef decltype(callback) ft; - Bind::value> binder; - boost::signals2::connection connection = binder.bindaux(signal, callback, object.get()).track(object); + // 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 || 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_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 3c67de63a..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,18 +14,25 @@ struct EvMyBehavior: sc::event> }; -class CbMySubscriberBehavior : public smacc::SmaccClientBehavior +class CbMySubscriberBehavior : public smacc::SmaccClientBehavior, public std::enable_shared_from_this { public: void onEntry() { - ROS_ERROR("ALLO"); std::weak_ptr client = this->requiresClient(); if(auto locked_client = client.lock()) { - ROS_ERROR("LOCKED"); - locked_client->onMessageReceived(&CbMySubscriberBehavior::onMessageReceived, this); + // 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()); } }