smacc_state_machine_impl.hpp

Go to the documentation of this file.

// Copyright 2025 Robosoft 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 <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()
{
  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());
  }
}
 
//-------------------------------------------------------------------------------------------------------
template <typename SmaccComponentType>
void ISmaccStateMachine::requiresComponent(
  SmaccComponentType *& storage, ComponentRequirement requirementType)
{
  bool throwsException = requirementType == ComponentRequirement::HARD;
  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");
}
//-------------------------------------------------------------------------------------------------------
template <typename EventType>
void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
{
  std::lock_guard<std::recursive_mutex> guard(eventQueueMutex_);
 
#define eventtypename demangleSymbol<EventType>().c_str()
 
  TRACETOOLS_TRACEPOINT(smacc2_event, eventtypename);
 
  {
    auto evinfo = SmaccEventInfo(TypeInfo::getTypeInfoFromType<EventType>());
    EventLabel<EventType>(evinfo.label);
 
    smacc2_msgs::msg::SmaccEvent event;
    event.event_type = evinfo.getEventTypeName();
    event.event_source = evinfo.getEventSourceName();
    event.event_object_tag = evinfo.getOrthogonalName();
    event.label = evinfo.label;
 
    if (this->eventsLogPub_)
    {
      this->eventsLogPub_->publish(event);
    }
  }
 
  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);
  if (!currentState_.empty())
  {
    propagateEventToStateReactors(currentState_.back(), 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();
}
 
namespace utils
{
template <int arity>
struct Bind
{
  template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
  smacc2::SmaccSignalConnection bindaux(
    TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object,
    std::shared_ptr<CallbackCounterSemaphore> callbackCounter);
};
 
template <>
struct Bind<1>
{
  template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
  smacc2::SmaccSignalConnection 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>
  smacc2::SmaccSignalConnection 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>
  smacc2::SmaccSignalConnection 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>
  smacc2::SmaccSignalConnection 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>
smacc2::SmaccSignalConnection 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;
  smacc2::SmaccSignalConnection 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 unknown lifetime "
      "behavior. An exception may occur 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 Machine] Initializing 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(), "Orthogonal 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::notifyOnStateExiting(StateType * state)
{
  stateMachineCurrentAction = StateMachineInternalAction::STATE_EXITING;
  auto fullname = demangleSymbol(typeid(StateType).name());
  RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
 
  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(), "Event generator 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