Program Listing for File smacc_state_machine_impl.hpp
↰ Return to documentation for file (/tmp/ws/src/smacc2/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp
)
// Copyright 2021 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**************************************************************************************************
*
* Authors: Pablo Inigo Blasco, Brett Aldrich
*
**************************************************************************************************/
#pragma once
#include <memory>
#include <sstream>
#include <string>
#include <smacc2/introspection/introspection.hpp>
#include <smacc2/smacc_client.hpp>
#include <smacc2/smacc_event_generator.hpp>
#include <smacc2/smacc_orthogonal.hpp>
#include <smacc2/smacc_signal_detector.hpp>
#include <smacc2/smacc_state.hpp>
#include <smacc2/smacc_state_machine.hpp>
#include <smacc2/smacc_state_reactor.hpp>
#include <smacc2/introspection/introspection.hpp>
#include <smacc2/smacc_client.hpp>
#include <smacc2/smacc_orthogonal.hpp>
#include <smacc2/smacc_signal_detector.hpp>
#include <smacc2/smacc_state.hpp>
#include <smacc2/smacc_state_machine.hpp>
#include <smacc2/smacc_state_reactor.hpp>
#include <boost/function_types/function_arity.hpp>
#include <boost/function_types/function_type.hpp>
#include <boost/function_types/parameter_types.hpp>
#include <smacc2/smacc_tracing/smacc_tracing.hpp>
#include <smacc2_msgs/msg/smacc_status.hpp>
namespace smacc2
{
using namespace smacc2::introspection;
template <typename TOrthogonal>
TOrthogonal * ISmaccStateMachine::getOrthogonal()
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
std::string orthogonalkey = demangledTypeName<TOrthogonal>();
TOrthogonal * ret;
auto it = orthogonals_.find(orthogonalkey);
if (it != orthogonals_.end())
{
RCLCPP_DEBUG(
getLogger(),
"Orthogonal %s resource is being required from some state, client or component. Found "
"resource in "
"cache.",
orthogonalkey.c_str());
ret = dynamic_cast<TOrthogonal *>(it->second.get());
return ret;
}
else
{
std::stringstream ss;
ss << "Orthogonal not found " << orthogonalkey.c_str() << std::endl;
ss << "The existing orthogonals are the following: " << std::endl;
for (auto & orthogonal : orthogonals_)
{
ss << " - " << orthogonal.first << std::endl;
}
RCLCPP_WARN_STREAM(getLogger(), ss.str());
return nullptr;
}
}
//-------------------------------------------------------------------------------------------------------
template <typename TOrthogonal>
void ISmaccStateMachine::createOrthogonal()
{
//this->lockStateMachine("create orthogonal");
std::lock_guard<std::recursive_mutex> guard(m_mutex_);
std::string orthogonalkey = demangledTypeName<TOrthogonal>();
if (orthogonals_.count(orthogonalkey) == 0)
{
auto ret = std::make_shared<TOrthogonal>();
orthogonals_[orthogonalkey] = dynamic_pointer_cast<smacc2::ISmaccOrthogonal>(ret);
ret->setStateMachine(this);
RCLCPP_INFO(getLogger(), "%s Orthogonal is created", orthogonalkey.c_str());
}
else
{
RCLCPP_WARN_STREAM(
getLogger(), "There were already one existing orthogonal of type "
<< orthogonalkey.c_str() << ". Skipping creation orthogonal request. ");
std::stringstream ss;
ss << "The existing orthogonals are the following: " << std::endl;
for (auto & orthogonal : orthogonals_)
{
ss << " - " << orthogonal.first << std::endl;
}
RCLCPP_WARN_STREAM(getLogger(), ss.str());
}
//this->unlockStateMachine("create orthogonal");
}
//-------------------------------------------------------------------------------------------------------
template <typename SmaccComponentType>
void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool throwsException)
{
RCLCPP_DEBUG(
getLogger(), "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;
}
}
}
RCLCPP_WARN(
getLogger(), "component %s is required but it was not found in any orthogonal",
demangleSymbol(typeid(SmaccComponentType).name()).c_str());
if (throwsException)
throw std::runtime_error("component is required but it was not found in any orthogonal");
// std::string componentkey = demangledTypeName<SmaccComponentType>();
// SmaccComponentType *ret;
// auto it = components_.find(componentkey);
// if (it == components_.end())
// {
// RCLCPP_DEBUG(getLogger(),"%s smacc component is required. Creating a new instance.",
// componentkey.c_str());
// ret = new SmaccComponentType();
// ret->setStateMachine(this);
// components_[componentkey] = static_cast<smacc2::ISmaccComponent *>(ret);
// RCLCPP_DEBUG(getLogger(),"%s resource is required. Done.", componentkey.c_str());
// }
// else
// {
// RCLCPP_DEBUG(getLogger(),"%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)
{
std::lock_guard<std::recursive_mutex> guard(eventQueueMutex_);
#define eventtypename demangleSymbol<EventType>().c_str()
TRACEPOINT(smacc2_event, eventtypename);
if (
evlifetime == EventLifeTime::CURRENT_STATE &&
(stateMachineCurrentAction == StateMachineInternalAction::STATE_EXITING ||
stateMachineCurrentAction == StateMachineInternalAction::TRANSITIONING))
{
RCLCPP_WARN_STREAM(
getLogger(),
"[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
<< eventtypename);
return;
// in this case we may lose/skip events, if this is not right for some cases we should create a
// queue to lock the events during the transitions. This issues appeared when a client
// asyncbehavior was posting an event meanwhile we were doing the transition, but the main
// thread was waiting for its correct finalization (with thread.join)
}
// when a postting event is requested by any component, client, or client behavior
// we reach this place. Now, we propagate the events to all the state state reactors to generate
// some more events
RCLCPP_DEBUG_STREAM(getLogger(), "[PostEvent entry point] " << eventtypename);
for (auto currentState : currentState_)
{
propagateEventToStateReactors(currentState, ev);
}
this->signalDetector_->postEvent(ev);
}
template <typename EventType>
void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
{
auto evname = smacc2::introspection::demangleSymbol<EventType>();
RCLCPP_INFO_STREAM(getLogger(), "Event " << evname);
auto * ev = new EventType();
this->postEvent(ev, evlifetime);
}
template <typename T>
bool ISmaccStateMachine::getGlobalSMData(std::string name, T & ret)
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
// RCLCPP_WARN(getLogger(),"get SM Data lock acquire");
bool success = false;
if (!globalData_.count(name))
{
// RCLCPP_WARN(getLogger(),"get SM Data - data do not exist");
success = false;
}
else
{
// RCLCPP_WARN(getLogger(),"get SM DAta -data exist. accessing");
try
{
auto & v = globalData_[name];
// RCLCPP_WARN(getLogger(),"get SM DAta -data exist. any cast");
ret = boost::any_cast<T>(v.second);
success = true;
// RCLCPP_WARN(getLogger(),"get SM DAta -data exist. success");
}
catch (boost::bad_any_cast & ex)
{
RCLCPP_ERROR(getLogger(), "bad any cast: %s", ex.what());
success = false;
}
}
// RCLCPP_WARN(getLogger(),"get SM Data lock release");
return success;
}
template <typename T>
void ISmaccStateMachine::setGlobalSMData(std::string name, T value)
{
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
// RCLCPP_WARN(getLogger(),"set SM Data lock acquire");
globalData_[name] = {
[this, name]()
{
std::stringstream ss;
auto val = any_cast<T>(globalData_[name].second);
ss << val;
return ss.str();
},
value};
}
this->updateStatusMessage();
}
template <typename StateField, typename BehaviorType>
void ISmaccStateMachine::mapBehavior()
{
std::string stateFieldName = demangleSymbol(typeid(StateField).name());
std::string behaviorType = demangleSymbol(typeid(BehaviorType).name());
RCLCPP_INFO(
getLogger(), "Mapping state field '%s' to stateReactor '%s'", stateFieldName.c_str(),
behaviorType.c_str());
SmaccClientBehavior * globalreference;
if (!this->getGlobalSMData(stateFieldName, globalreference))
{
// Using the requires component approach, we force a unique existence
// of this component
BehaviorType * behavior;
this->requiresComponent(behavior);
globalreference = dynamic_cast<ISmaccClientBehavior *>(behavior);
this->setGlobalSMData(stateFieldName, globalreference);
}
}
namespace utils
{
template <int arity>
struct Bind
{
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection bindaux(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
std::shared_ptr<CallbackCounterSemaphore> callbackCounter);
};
template <>
struct Bind<1>
{
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection bindaux(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
{
return signal.connect(
[=]()
{
if (callbackCounter == nullptr)
{
(object->*callback)();
}
else if (callbackCounter->acquire())
{
(object->*callback)();
callbackCounter->release();
}
});
}
};
template <>
struct Bind<2>
{
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection bindaux(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
{
return signal.connect(
[=](auto a1)
{
if (callbackCounter == nullptr)
{
return (object->*callback)(a1);
}
else if (callbackCounter->acquire())
{
(object->*callback)(a1);
callbackCounter->release();
}
});
}
};
template <>
struct Bind<3>
{
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection bindaux(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
{
return signal.connect(
[=](auto a1, auto a2)
{
if (callbackCounter == nullptr)
{
return (object->*callback)(a1, a2);
}
else if (callbackCounter->acquire())
{
(object->*callback)(a1, a2);
callbackCounter->release();
}
});
}
};
template <>
struct Bind<4>
{
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection bindaux(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
std::shared_ptr<CallbackCounterSemaphore> callbackCounter)
{
return signal.connect(
[=](auto a1, auto a2, auto a3)
{
if (callbackCounter == nullptr)
{
return (object->*callback)(a1, a2, a3);
}
else if (callbackCounter->acquire())
{
(object->*callback)(a1, a2, a3);
callbackCounter->release();
}
});
}
};
} // namespace utils
using namespace smacc2::utils;
template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
boost::signals2::connection ISmaccStateMachine::createSignalConnection(
TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object)
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
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;
Bind<boost::function_types::function_arity<ft>::value> binder;
boost::signals2::connection connection;
// 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)
{
RCLCPP_INFO(
getLogger(),
"[StateMachine] long life-time smacc signal subscription created. Subscriber is %s. Callback "
"is: %s",
demangledTypeName<TSmaccObjectType>().c_str(),
demangleSymbol(typeid(callback).name()).c_str());
connection = binder.bindaux(signal, callback, object, nullptr);
}
else if (
std::is_base_of<ISmaccState, TSmaccObjectType>::value ||
std::is_base_of<StateReactor, TSmaccObjectType>::value ||
std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
{
RCLCPP_INFO(
getLogger(),
"[StateMachine] life-time constrained smacc signal subscription created. Subscriber is %s",
demangledTypeName<TSmaccObjectType>().c_str());
std::shared_ptr<CallbackCounterSemaphore> callbackCounterSemaphore;
if (stateCallbackConnections.count(object))
{
callbackCounterSemaphore = stateCallbackConnections[object];
}
else
{
callbackCounterSemaphore =
std::make_shared<CallbackCounterSemaphore>(demangledTypeName<TSmaccObjectType>().c_str());
stateCallbackConnections[object] = callbackCounterSemaphore;
}
connection = binder.bindaux(signal, callback, object, callbackCounterSemaphore);
callbackCounterSemaphore->addConnection(connection);
}
else // state life-time objects
{
RCLCPP_WARN(
getLogger(),
"[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.");
connection = binder.bindaux(signal, callback, object, nullptr);
}
return connection;
}
template <typename StateType>
void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
RCLCPP_DEBUG(
getLogger(),
"[State Machne] Initializating a new state '%s' and updating current state. Getting state "
"meta-information. number of orthogonals: %ld",
demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
stateSeqCounter_++;
currentState_.push_back(state);
currentStateInfo_ = stateMachineInfo_->getState<StateType>();
}
template <typename StateType>
void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
{
RCLCPP_INFO(
getLogger(), "[%s] State OnEntry code finished",
demangleSymbol(typeid(StateType).name()).c_str());
auto currentState = this->currentState_.back();
for (auto pair : this->orthogonals_)
{
RCLCPP_DEBUG(getLogger(), "ortho onentry: %s", pair.second->getName().c_str());
auto & orthogonal = pair.second;
try
{
orthogonal->onEntry();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception on Entry - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : currentState->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
RCLCPP_INFO_STREAM(getLogger(), "state reactor onEntry: " << srname);
try
{
sr->onEntry();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on Entry - continuing with next state reactor. Exception "
"info: %s",
srname.c_str(), e.what());
}
}
for (auto & eg : currentState->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
RCLCPP_INFO_STREAM(getLogger(), "event generator onEntry: " << egname);
try
{
eg->onEntry();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Event generator %s] Exception on Entry - continuing with next state reactor. Exception "
"info: %s",
egname.c_str(), e.what());
}
}
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
this->updateStatusMessage();
stateMachineCurrentAction = StateMachineInternalAction::STATE_RUNNING;
}
}
template <typename StateType>
void ISmaccStateMachine::notifyOnRuntimeConfigurationFinished(StateType *)
{
for (auto pair : this->orthogonals_)
{
// RCLCPP_INFO(getLogger(),"ortho onruntime configure: %s", pair.second->getName().c_str());
auto & orthogonal = pair.second;
orthogonal->runtimeConfigure();
}
{
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
this->updateStatusMessage();
this->signalDetector_->notifyStateConfigured(this->currentState_.back());
stateMachineCurrentAction = StateMachineInternalAction::STATE_ENTERING;
}
}
template <typename StateType>
void ISmaccStateMachine::notifyOnRuntimeConfigured(StateType *)
{
stateMachineCurrentAction = StateMachineInternalAction::STATE_CONFIGURING;
}
template <typename StateType>
void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
{
stateMachineCurrentAction = StateMachineInternalAction::STATE_EXITING;
auto fullname = demangleSymbol(typeid(StateType).name());
RCLCPP_WARN_STREAM(getLogger(), "exiting state: " << fullname);
// this->set_parameter("destroyed", true);
RCLCPP_INFO_STREAM(getLogger(), "Notification State Exit: leaving state " << state);
for (auto pair : this->orthogonals_)
{
auto & orthogonal = pair.second;
try
{
orthogonal->onExit();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception onExit - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : state->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
RCLCPP_INFO_STREAM(getLogger(), "state reactor OnExit: " << srname);
try
{
sr->onExit();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
"info: %s",
srname.c_str(), e.what());
}
}
for (auto & eg : state->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
RCLCPP_INFO_STREAM(getLogger(), "event generator OnExit: " << egname);
try
{
eg->onExit();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
"info: %s",
egname.c_str(), e.what());
}
}
}
template <typename StateType>
void ISmaccStateMachine::notifyOnStateExited(StateType * state)
{
this->lockStateMachine("state exit");
signalDetector_->notifyStateExited(state);
auto fullname = demangleSymbol(typeid(StateType).name());
RCLCPP_WARN_STREAM(getLogger(), "exiting state: " << fullname);
RCLCPP_INFO_STREAM(getLogger(), "Notification State Disposing: leaving state" << state);
for (auto pair : this->orthogonals_)
{
auto & orthogonal = pair.second;
try
{
orthogonal->onDispose();
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[Orthogonal %s] Exception onDispose - continuing with next orthogonal. Exception info: %s",
pair.second->getName().c_str(), e.what());
}
}
for (auto & sr : state->getStateReactors())
{
auto srname = smacc2::demangleSymbol(typeid(*sr).name()).c_str();
RCLCPP_INFO(getLogger(), "state reactor disposing: %s", srname);
try
{
this->disconnectSmaccSignalObject(sr.get());
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
"info: %s",
srname, e.what());
}
}
for (auto & eg : state->getEventGenerators())
{
auto egname = smacc2::demangleSymbol(typeid(*eg).name()).c_str();
RCLCPP_INFO(getLogger(), "state reactor disposing: %s", egname);
try
{
this->disconnectSmaccSignalObject(eg.get());
}
catch (const std::exception & e)
{
RCLCPP_ERROR(
getLogger(),
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
"info: %s",
egname, e.what());
}
}
this->stateCallbackConnections.clear();
currentState_.pop_back();
// then call exit state
RCLCPP_WARN_STREAM(getLogger(), "state exit: " << fullname);
stateMachineCurrentAction = StateMachineInternalAction::TRANSITIONING;
this->unlockStateMachine("state exit");
}
//------------------------------------------------------------------------------------------------
template <typename EventType>
void ISmaccStateMachine::propagateEventToStateReactors(ISmaccState * st, EventType * ev)
{
RCLCPP_DEBUG(
getLogger(), "PROPAGATING EVENT [%s] TO SRs [%s]: ", demangleSymbol<EventType>().c_str(),
st->getClassName().c_str());
for (auto & sb : st->getStateReactors())
{
sb->notifyEvent(ev);
}
auto * pst = st->getParentState();
if (pst != nullptr)
{
propagateEventToStateReactors(pst, ev);
}
}
template <typename InitialStateType>
void ISmaccStateMachine::buildStateMachineInfo()
{
this->stateMachineInfo_ = std::make_shared<SmaccStateMachineInfo>(this->getNode());
this->stateMachineInfo_->buildStateMachineInfo<InitialStateType>();
this->stateMachineInfo_->assembleSMStructureMessage(this);
this->checkStateMachineConsistence();
}
int64_t ISmaccStateMachine::getCurrentStateCounter() const { return this->stateSeqCounter_; }
ISmaccState * ISmaccStateMachine::getCurrentState() const { return this->currentState_.back(); }
const smacc2::introspection::SmaccStateMachineInfo & ISmaccStateMachine::getStateMachineInfo()
{
return *this->stateMachineInfo_;
}
} // namespace smacc2