44#include <boost/function_types/function_arity.hpp>
45#include <boost/function_types/function_type.hpp>
46#include <boost/function_types/parameter_types.hpp>
48#include <smacc2_msgs/msg/smacc_status.hpp>
54template <
typename TOrthogonal>
57 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
68 "Orthogonal %s resource is being required from some state, client or component. Found "
71 orthogonalkey.c_str());
72 ret =
dynamic_cast<TOrthogonal *
>(it->second.get());
78 ss <<
"Orthogonal not found " << orthogonalkey.c_str() << std::endl;
79 ss <<
"The existing orthogonals are the following: " << std::endl;
82 ss <<
" - " << orthogonal.first << std::endl;
85 RCLCPP_WARN_STREAM(
getLogger(), ss.str());
92template <
typename TOrthogonal>
96 std::lock_guard<std::recursive_mutex> guard(
m_mutex_);
102 auto ret = std::make_shared<TOrthogonal>();
103 orthogonals_[orthogonalkey] = dynamic_pointer_cast<smacc2::ISmaccOrthogonal>(ret);
105 ret->setStateMachine(
this);
107 RCLCPP_INFO(
getLogger(),
"%s Orthogonal is created", orthogonalkey.c_str());
112 getLogger(),
"There were already one existing orthogonal of type "
113 << orthogonalkey.c_str() <<
". Skipping creation orthogonal request. ");
114 std::stringstream ss;
115 ss <<
"The existing orthogonals are the following: " << std::endl;
118 ss <<
" - " << orthogonal.first << std::endl;
120 RCLCPP_WARN_STREAM(
getLogger(), ss.str());
126template <
typename SmaccComponentType>
132 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
136 for (
auto & client : ortho.second->clients_)
138 storage = client->getComponent<SmaccComponentType>();
139 if (storage !=
nullptr)
147 getLogger(),
"component %s is required but it was not found in any orthogonal",
151 throw std::runtime_error(
"component is required but it was not found in any orthogonal");
177template <
typename EventType>
182#define eventtypename demangleSymbol<EventType>().c_str()
190 smacc2_msgs::msg::SmaccEvent event;
191 event.event_type = evinfo.getEventTypeName();
192 event.event_source = evinfo.getEventSourceName();
193 event.event_object_tag = evinfo.getOrthogonalName();
194 event.label = evinfo.label;
206 "[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
228template <
typename EventType>
232 RCLCPP_INFO_STREAM(
getLogger(),
"Event: " << evname);
233 auto * ev =
new EventType();
240 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
242 bool success =
false;
257 ret = boost::any_cast<T>(v.second);
261 catch (boost::bad_any_cast & ex)
263 RCLCPP_ERROR(
getLogger(),
"bad any cast: %s", ex.what());
276 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
282 std::stringstream ss;
319 template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
321 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object,
322 std::shared_ptr<CallbackCounterSemaphore> callbackCounter);
328 template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
330 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object,
331 std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
333 return signal.connect(
337 if (callbackCounter ==
nullptr)
339 (
object->*callback)();
341 else if (callbackCounter->acquire())
343 (
object->*callback)();
344 callbackCounter->release();
353 template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
355 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object,
356 std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
358 return signal.connect(
361 if (callbackCounter ==
nullptr)
363 return (object->*callback)(a1);
365 else if (callbackCounter->acquire())
367 (
object->*callback)(a1);
368 callbackCounter->release();
377 template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
379 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object,
380 std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
382 return signal.connect(
383 [=](
auto a1,
auto a2)
385 if (callbackCounter ==
nullptr)
387 return (object->*callback)(a1, a2);
389 else if (callbackCounter->acquire())
391 (
object->*callback)(a1, a2);
392 callbackCounter->release();
401 template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
403 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object,
404 std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
406 return signal.connect(
407 [=](
auto a1,
auto a2,
auto a3)
409 if (callbackCounter ==
nullptr)
411 return (object->*callback)(a1, a2, a3);
413 else if (callbackCounter->acquire())
415 (
object->*callback)(a1, a2, a3);
416 callbackCounter->release();
424template <
typename TSmaccSignal,
typename TMemberFunctionPrototype,
typename TSmaccObjectType>
426 TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType *
object)
428 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
431 std::is_base_of<ISmaccState, TSmaccObjectType>::value ||
432 std::is_base_of<ISmaccClient, TSmaccObjectType>::value ||
433 std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value ||
434 std::is_base_of<StateReactor, TSmaccObjectType>::value ||
435 std::is_base_of<ISmaccComponent, TSmaccObjectType>::value,
436 "Only are accepted smacc types as subscribers for smacc signals");
438 typedef decltype(callback) ft;
440 boost::signals2::connection connection;
444 std::is_base_of<ISmaccComponent, TSmaccObjectType>::value ||
445 std::is_base_of<ISmaccClient, TSmaccObjectType>::value ||
446 std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
447 std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
451 "[StateMachine] Long life-time SMACC signal subscription created. Subscriber is %s. Callback "
456 connection = binder.bindaux(signal, callback,
object,
nullptr);
459 std::is_base_of<ISmaccState, TSmaccObjectType>::value ||
460 std::is_base_of<StateReactor, TSmaccObjectType>::value ||
461 std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
465 "[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is %s",
468 std::shared_ptr<CallbackCounterSemaphore> callbackCounterSemaphore;
475 callbackCounterSemaphore =
480 connection = binder.bindaux(signal, callback,
object, callbackCounterSemaphore);
481 callbackCounterSemaphore->addConnection(connection);
487 "[StateMachine] Connecting signal to an unknown object with unknown lifetime "
488 "behavior. An exception may occur if the object is destroyed during the execution.");
490 connection = binder.bindaux(signal, callback,
object,
nullptr);
496template <
typename StateType>
499 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
503 "[State Machine] Initializing a new state '%s' and updating current state. Getting state "
504 "meta-information. number of orthogonals: %ld",
512template <
typename StateType>
516 getLogger(),
"[%s] State OnEntry code finished.",
522 RCLCPP_DEBUG(
getLogger(),
"Orthogonal onEntry: %s.", pair.second->getName().c_str());
523 auto & orthogonal = pair.second;
526 orthogonal->onEntry();
528 catch (
const std::exception & e)
532 "[Orthogonal %s] Exception on Entry - continuing with next orthogonal. Exception info: %s",
533 pair.second->getName().c_str(), e.what());
537 for (
auto & sr : currentState->getStateReactors())
540 RCLCPP_INFO_STREAM(
getLogger(),
"State reactor onEntry: " << srname);
545 catch (
const std::exception & e)
549 "[State Reactor %s] Exception on Entry - continuing with next state reactor. Exception "
551 srname.c_str(), e.what());
555 for (
auto & eg : currentState->getEventGenerators())
558 RCLCPP_INFO_STREAM(
getLogger(),
"Event generator onEntry: " << egname);
563 catch (
const std::exception & e)
567 "[Event generator %s] Exception on Entry - continuing with next state reactor. Exception "
569 egname.c_str(), e.what());
574 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
580template <
typename StateType>
586 auto & orthogonal = pair.second;
587 orthogonal->runtimeConfigure();
591 std::lock_guard<std::recursive_mutex> lock(
m_mutex_);
599template <
typename StateType>
605template <
typename StateType>
610 RCLCPP_WARN_STREAM(
getLogger(),
"Exiting state: " << fullname);
613 RCLCPP_INFO_STREAM(
getLogger(),
"Notification state exit: leaving state " << state);
616 auto & orthogonal = pair.second;
619 orthogonal->onExit();
621 catch (
const std::exception & e)
625 "[Orthogonal %s] Exception onExit - continuing with next orthogonal. Exception info: %s",
626 pair.second->getName().c_str(), e.what());
630 for (
auto & sr : state->getStateReactors())
633 RCLCPP_INFO_STREAM(
getLogger(),
"State reactor OnExit: " << srname);
638 catch (
const std::exception & e)
642 "[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
644 srname.c_str(), e.what());
648 for (
auto & eg : state->getEventGenerators())
651 RCLCPP_INFO_STREAM(
getLogger(),
"Event generator OnExit: " << egname);
656 catch (
const std::exception & e)
660 "[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
662 egname.c_str(), e.what());
667template <
typename StateType>
675 RCLCPP_WARN_STREAM(
getLogger(),
"Exiting state: " << fullname);
677 RCLCPP_INFO_STREAM(
getLogger(),
"Notification state disposing: leaving state" << state);
680 auto & orthogonal = pair.second;
683 orthogonal->onDispose();
685 catch (
const std::exception & e)
689 "[Orthogonal %s] Exception onDispose - continuing with next orthogonal. Exception info: %s",
690 pair.second->getName().c_str(), e.what());
694 for (
auto & sr : state->getStateReactors())
697 RCLCPP_INFO(
getLogger(),
"State reactor disposing: %s", srname);
702 catch (
const std::exception & e)
706 "[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
712 for (
auto & eg : state->getEventGenerators())
715 RCLCPP_INFO(
getLogger(),
"Event generator disposing: %s", egname);
720 catch (
const std::exception & e)
724 "[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
734 RCLCPP_WARN_STREAM(
getLogger(),
"State exit: " << fullname);
740template <
typename EventType>
758template <
typename InitialStateType>
std::map< std::string, std::shared_ptr< smacc2::ISmaccOrthogonal > > orthogonals_
std::vector< ISmaccState * > currentState_
std::recursive_mutex eventQueueMutex_
bool getGlobalSMData(std::string name, T &ret)
boost::signals2::connection createSignalConnection(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object)
StateMachineInternalAction stateMachineCurrentAction
std::shared_ptr< SmaccStateInfo > currentStateInfo_
rclcpp::Node::SharedPtr getNode()
std::map< std::string, std::pair< std::function< std::string()>, boost::any > > globalData_
void notifyOnStateExitting(StateType *state)
void setGlobalSMData(std::string name, T value)
TOrthogonal * getOrthogonal()
void buildStateMachineInfo()
void notifyOnRuntimeConfigurationFinished(StateType *state)
std::recursive_mutex m_mutex_
void notifyOnStateExited(StateType *state)
void lockStateMachine(std::string msg)
void notifyOnStateEntryEnd(StateType *state)
void propagateEventToStateReactors(ISmaccState *st, EventType *ev)
void disconnectSmaccSignalObject(void *object)
const SmaccStateMachineInfo & getStateMachineInfo()
void unlockStateMachine(std::string msg)
int64_t getCurrentStateCounter() const
void requiresComponent(SmaccComponentType *&storage, bool throwsExceptionIfNotExist=false)
ISmaccState * getCurrentState() const
void checkStateMachineConsistence()
SignalDetector * signalDetector_
std::map< void *, std::shared_ptr< CallbackCounterSemaphore > > stateCallbackConnections
rclcpp::Logger getLogger()
void notifyOnRuntimeConfigured(StateType *state)
std::shared_ptr< SmaccStateMachineInfo > stateMachineInfo_
void postEvent(EventType *ev, EventLifeTime evlifetime=EventLifeTime::ABSOLUTE)
rclcpp::Publisher< smacc2_msgs::msg::SmaccEvent >::SharedPtr eventsLogPub_
void updateStatusMessage()
void notifyOnStateEntryStart(StateType *state)
ISmaccState * getParentState()
virtual std::string getClassName()
std::vector< std::shared_ptr< StateReactor > > & getStateReactors()
void postEvent(EventType *ev)
void notifyStateExited(ISmaccState *currentState)
void notifyStateConfigured(ISmaccState *currentState)
static TypeInfo::Ptr getTypeInfoFromType()
std::enable_if< HasEventLabel< T >::value, void >::type EventLabel(std::string &label)
std::string demangleSymbol()
std::string demangledTypeName()
void TRACEPOINT(spinOnce)
boost::signals2::connection bindaux(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object, std::shared_ptr< CallbackCounterSemaphore > callbackCounter)
boost::signals2::connection bindaux(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object, std::shared_ptr< CallbackCounterSemaphore > callbackCounter)
boost::signals2::connection bindaux(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object, std::shared_ptr< CallbackCounterSemaphore > callbackCounter)
boost::signals2::connection bindaux(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object, std::shared_ptr< CallbackCounterSemaphore > callbackCounter)
boost::signals2::connection bindaux(TSmaccSignal &signal, TMemberFunctionPrototype callback, TSmaccObjectType *object, std::shared_ptr< CallbackCounterSemaphore > callbackCounter)