#include <smacc_signal_detector.hpp>

Collaboration diagram for smacc2::SignalDetector:

Public Member Functions
	SignalDetector (SmaccFifoScheduler *scheduler, ExecutionModel executionModel=ExecutionModel::SINGLE_THREAD_SPINNER)

void	initialize (ISmaccStateMachine *stateMachine)

void	setProcessorHandle (SmaccFifoScheduler::processor_handle processorHandle)

void	runThread ()

void	join ()

void	stop ()

void	terminateScheduler ()

void	pollingLoop ()

template<typename EventType >
void	postEvent (EventType *ev)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger ()

void	notifyStateConfigured (ISmaccState *currentState)

void	notifyStateExited (ISmaccState *currentState)

void	notifyRosInitialized ()

Private Member Functions
void	pollOnce ()

void	findUpdatableClientsAndComponents ()

void	findUpdatableStateElements (ISmaccState *currentState)

Private Attributes
ISmaccStateMachine *	smaccStateMachine_

std::vector< ISmaccUpdatable * >	updatableClients_

std::vector< std::vector< ISmaccUpdatable * > >	updatableStateElements_

std::atomic< int64_t >	lastState_

double	loop_rate_hz

std::atomic< bool >	end_

std::atomic< bool >	initialized_

std::atomic< bool >	rosInitialized_

rclcpp::Publisher< smacc2_msgs::msg::SmaccStatus >::SharedPtr	statusPub_

SmaccFifoScheduler *	scheduler_

SmaccFifoScheduler::processor_handle	processorHandle_

boost::thread	signalDetectorThread_

ExecutionModel	executionModel_

Detailed Description

Definition at line 37 of file smacc_signal_detector.hpp.

Constructor & Destructor Documentation

◆ SignalDetector()

smacc2::SignalDetector::SignalDetector	(	SmaccFifoScheduler *	scheduler,
		ExecutionModel	executionModel = ExecutionModel::SINGLE_THREAD_SPINNER )

SignalDetector()

Definition at line 64 of file signal_detector.cpp.

{
  scheduler_ = scheduler;
  loop_rate_hz = 20.0;
  end_ = false;
  initialized_ = false;
  rosInitialized_ = false;
  executionModel_ = executionModel;
}

References end_, executionModel_, initialized_, loop_rate_hz, rosInitialized_, and scheduler_.

Member Function Documentation

◆ findUpdatableClientsAndComponents()

void smacc2::SignalDetector::findUpdatableClientsAndComponents ( )

private

findUpdatableClientsAndComponents()

Definition at line 96 of file signal_detector.cpp.

{
  this->updatableClients_.clear();
  for (auto pair : this->smaccStateMachine_->getOrthogonals())
  {
    auto & orthogonal = pair.second;
    auto & clients = orthogonal->getClients();
 
    for (auto & client : clients)
    {
      // updatable client components
      auto updatableClient = dynamic_cast<ISmaccUpdatable *>(client.get());
 
      if (updatableClient != nullptr)
      {
        RCLCPP_DEBUG_STREAM(
          getLogger(), "Adding updatable client: " << demangleType(typeid(updatableClient)));
        this->updatableClients_.push_back(updatableClient);
      }
 
      // updatable client components
      std::vector<std::shared_ptr<ISmaccComponent>> components;
      client->getComponents(components);
      for (auto & componententry : components)
      {
        auto updatableComponent = dynamic_cast<ISmaccUpdatable *>(componententry.get());
        if (updatableComponent != nullptr)
        {
          RCLCPP_DEBUG_STREAM(
            getLogger(),
            "Adding updatable component: " << demangleType(typeid(*updatableComponent)));
          this->updatableClients_.push_back(updatableComponent);
        }
      }
    }
  }
}

References smacc2::introspection::demangleType(), getLogger(), smacc2::ISmaccStateMachine::getOrthogonals(), smaccStateMachine_, and updatableClients_.

Referenced by initialize(), and pollOnce().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ findUpdatableStateElements()

void smacc2::SignalDetector::findUpdatableStateElements ( ISmaccState * currentState )

private

findUpdatableClientBehaviors()

Definition at line 139 of file signal_detector.cpp.

{
  updatableStateElements_.push_back(std::vector<ISmaccUpdatable *>());
 
  auto & updatableElements = updatableStateElements_.back();
 
  for (auto pair : this->smaccStateMachine_->getOrthogonals())
  {
    auto & orthogonal = pair.second;
    auto & behaviors = orthogonal->getClientBehaviors().back();
 
    for (auto & currentBehavior : behaviors)
    {
      ISmaccUpdatable * updatableClientBehavior =
        dynamic_cast<ISmaccUpdatable *>(currentBehavior.get());
 
      if (updatableClientBehavior != nullptr)
      {
        RCLCPP_DEBUG_STREAM(
          getLogger(),
          "Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)));
        updatableElements.push_back(updatableClientBehavior);
      }
    }
  }
 
  auto updatableState = dynamic_cast<ISmaccUpdatable *>(currentState);
  if (updatableState != nullptr)
  {
    updatableElements.push_back(updatableState);
  }
 
  auto statereactors = currentState->getStateReactors();
  for (auto & sr : statereactors)
  {
    ISmaccUpdatable * updatableStateReactor = dynamic_cast<ISmaccUpdatable *>(sr.get());
    if (updatableStateReactor != nullptr)
    {
      RCLCPP_DEBUG_STREAM(
        getLogger(),
        "Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)));
      updatableElements.push_back(updatableStateReactor);
    }
  }
 
  auto eventgenerators = currentState->getEventGenerators();
  for (auto & eg : eventgenerators)
  {
    ISmaccUpdatable * updatableEventGenerator = dynamic_cast<ISmaccUpdatable *>(eg.get());
    if (updatableEventGenerator != nullptr)
    {
      RCLCPP_DEBUG_STREAM(
        getLogger(),
        "Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)));
      updatableElements.push_back(updatableEventGenerator);
    }
  }
}

References smacc2::introspection::demangleType(), smacc2::ISmaccState::getEventGenerators(), getLogger(), smacc2::ISmaccStateMachine::getOrthogonals(), smacc2::ISmaccState::getStateReactors(), smaccStateMachine_, and updatableStateElements_.

Referenced by notifyStateConfigured().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getLogger()

rclcpp::Logger smacc2::SignalDetector::getLogger ( )

inline

Definition at line 68 of file smacc_signal_detector.hpp.

68{ return getNode()->get_logger(); }

smacc2::SignalDetector::getNode

rclcpp::Node::SharedPtr getNode()

Definition signal_detector.cpp:74

References getNode().

Referenced by findUpdatableClientsAndComponents(), findUpdatableStateElements(), notifyRosInitialized(), pollingLoop(), pollOnce(), and terminateScheduler().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getNode()

rclcpp::Node::SharedPtr smacc2::SignalDetector::getNode ( )

Definition at line 74 of file signal_detector.cpp.

74{ return this->smaccStateMachine_->getNode(); }

smacc2::ISmaccStateMachine::getNode

rclcpp::Node::SharedPtr getNode()

Definition smacc_state_machine.cpp:94

References smacc2::ISmaccStateMachine::getNode(), and smaccStateMachine_.

Referenced by getLogger(), initialize(), pollingLoop(), and pollOnce().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initialize()

void smacc2::SignalDetector::initialize ( ISmaccStateMachine * stateMachine )

initialize()

Definition at line 81 of file signal_detector.cpp.

{
  smaccStateMachine_ = stateMachine;
  lastState_ = std::numeric_limits<int64_t>::quiet_NaN();
  findUpdatableClientsAndComponents();
  this->getNode()->declare_parameter("signal_detector_loop_freq", this->loop_rate_hz);
 
  initialized_ = true;
}

References findUpdatableClientsAndComponents(), getNode(), initialized_, lastState_, loop_rate_hz, and smaccStateMachine_.

Referenced by smacc2::ISmaccStateMachine::ISmaccStateMachine().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ join()

void smacc2::SignalDetector::join ( )

join()

Definition at line 239 of file signal_detector.cpp.

239{ signalDetectorThread_.join(); }

smacc2::SignalDetector::signalDetectorThread_

boost::thread signalDetectorThread_

Definition smacc_signal_detector.hpp:110

References signalDetectorThread_.

◆ notifyRosInitialized()

void smacc2::SignalDetector::notifyRosInitialized ( )

Definition at line 208 of file signal_detector.cpp.

{
  RCLCPP_INFO(getLogger(), "[SignalDetector] ROS initialization complete, enabling polling loop");
  rosInitialized_ = true;
}

References getLogger(), and rosInitialized_.

Referenced by smacc2::ISmaccStateMachine::initializeROS().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ notifyStateConfigured()

void smacc2::SignalDetector::notifyStateConfigured ( ISmaccState * currentState )

Definition at line 198 of file signal_detector.cpp.

{
  this->findUpdatableStateElements(currentState);
}

References findUpdatableStateElements().

Referenced by smacc2::ISmaccStateMachine::notifyOnRuntimeConfigurationFinished().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ notifyStateExited()

void smacc2::SignalDetector::notifyStateExited ( ISmaccState * currentState )

Definition at line 203 of file signal_detector.cpp.

{
  this->updatableStateElements_.pop_back();
}

References updatableStateElements_.

Referenced by smacc2::ISmaccStateMachine::notifyOnStateExited().

Here is the caller graph for this function:

◆ pollingLoop()

void smacc2::SignalDetector::pollingLoop ( )

pollingLoop()

Definition at line 354 of file signal_detector.cpp.

{
  // rclcpp::Node::SharedPtr nh("~"); // use node name as root of the parameter server
  rclcpp::Node::SharedPtr _;
  rclcpp::Rate r0(20);
 
  // Wait for both SignalDetector::initialize() (called from ISmaccStateMachine constructor)
  // and ROS initialization to complete (initializeROS called from initiate_impl).
  // This ensures orthogonals, clients, and ROS objects are fully initialized
  // before we start polling and accessing them.
  while ((!initialized_ || !rosInitialized_) && rclcpp::ok() && !end_)
  {
    r0.sleep();
  }
 
  if (!rclcpp::ok() || end_)
  {
    RCLCPP_INFO(getLogger(), "[SignalDetector] Shutdown requested before initialization completed");
    return;
  }
 
  auto nh = getNode();
 
  if (!nh->get_parameter("signal_detector_loop_freq", this->loop_rate_hz))
  {
    RCLCPP_WARN(
      getLogger(),
      "Signal Detector frequency (ros param signal_detector_loop_freq) was not set, using default "
      "frequency: "
      "%lf",
      this->loop_rate_hz);
  }
  else
  {
    RCLCPP_WARN(
      getLogger(), "Signal Detector frequency (ros param signal_detector_loop_freq): %lf",
      this->loop_rate_hz);
  }
 
  nh->set_parameter(rclcpp::Parameter("signal_detector_loop_freq", this->loop_rate_hz));
 
  RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Loop rate hz:" << loop_rate_hz);
 
  if (this->executionModel_ == ExecutionModel::SINGLE_THREAD_SPINNER)
  {
    RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in single-threaded mode.");
 
    rclcpp::Rate r(loop_rate_hz);
    while (rclcpp::ok() && !end_)
    {
      RCLCPP_INFO_STREAM_THROTTLE(
        getLogger(), *getNode()->get_clock(), 10000, "[SignalDetector] Heartbeat");
      pollOnce();
      rclcpp::spin_some(nh);
      r.sleep();
    }
  }
  else
  {
    RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in multi-threaded mode.");
 
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(nh);
    executor.spin();
  }
}

References end_, executionModel_, getLogger(), getNode(), initialized_, loop_rate_hz, pollOnce(), rosInitialized_, and smacc2::SINGLE_THREAD_SPINNER.

Referenced by smacc2::run(), smacc2::run_async(), and runThread().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pollOnce()

void smacc2::SignalDetector::pollOnce ( )

private

poll()

Definition at line 267 of file signal_detector.cpp.

{
  // precondition: smaccStateMachine_ != nullptr
 
  //TRACETOOLS_TRACEPOINT( spinOnce);
  TRACETOOLS_TRACEPOINT(spinOnce);
 
  std::lock_guard<std::recursive_mutex> lock(smaccStateMachine_->m_mutex_);
  try
  {
    //smaccStateMachine_->lockStateMachine("update behaviors");
 
    this->findUpdatableClientsAndComponents();
    RCLCPP_DEBUG_STREAM(getLogger(), "Updatable clients: " << this->updatableClients_.size());
 
    if (this->updatableClients_.size())
    {
      auto node = getNode();
      for (auto * updatableClient : this->updatableClients_)
      {
        auto updatableElementName = demangleType(typeid(*updatableClient)).c_str();
        try
        {
          RCLCPP_DEBUG_STREAM(
            node->get_logger(),
            "[PollOnce] update client call:  " << demangleType(typeid(*updatableClient)));
 
          TRACETOOLS_TRACEPOINT(smacc2_state_update_start, updatableElementName);
          updatableClient->executeUpdate(smaccStateMachine_->getNode());
          TRACETOOLS_TRACEPOINT(smacc2_state_update_start, updatableElementName);
        }
        catch (const std::exception & e)
        {
          RCLCPP_ERROR_STREAM(
            node->get_logger(),
            "Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n');
        }
      }
    }
 
    // STATE UPDATABLE ELEMENTS
    if (
      this->smaccStateMachine_->stateMachineCurrentAction !=
        StateMachineInternalAction::TRANSITIONING &&
      this->smaccStateMachine_->stateMachineCurrentAction !=
        StateMachineInternalAction::STATE_CONFIGURING &&
      this->smaccStateMachine_->stateMachineCurrentAction !=
        StateMachineInternalAction::STATE_ENTERING &&
      this->smaccStateMachine_->stateMachineCurrentAction !=
        StateMachineInternalAction::STATE_EXITING)
    {
      RCLCPP_DEBUG_STREAM(
        getLogger(), "Updatable states: " << this->updatableStateElements_.size());
 
      for (auto stateElement : this->updatableStateElements_)
      {
        for (auto * udpatableStateElement : stateElement)
        {
          std::string updatableElementName = demangleType(typeid(*udpatableStateElement));
          auto updatableElementNameCstr = updatableElementName.c_str();
 
          RCLCPP_DEBUG_STREAM(
            getLogger(), "pollOnce update client behavior call: "
                           << demangleType(typeid(*udpatableStateElement)));
          TRACETOOLS_TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
 
          udpatableStateElement->executeUpdate(smaccStateMachine_->getNode());
          TRACETOOLS_TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
        }
      }
    }
  }
  catch (std::exception & ex)
  {
    RCLCPP_ERROR(getLogger(), "Exception during Signal Detector update loop. %s.", ex.what());
  }
 
  auto nh = this->getNode();
  rclcpp::spin_some(nh);
  //smaccStateMachine_->unlockStateMachine("update behaviors");
}

References smacc2::introspection::demangleType(), findUpdatableClientsAndComponents(), getLogger(), smacc2::ISmaccStateMachine::getNode(), getNode(), smacc2::ISmaccStateMachine::m_mutex_, smacc2_state_update_start, smaccStateMachine_, smacc2::STATE_CONFIGURING, smacc2::STATE_ENTERING, smacc2::STATE_EXITING, smacc2::ISmaccStateMachine::stateMachineCurrentAction, smacc2::TRANSITIONING, updatableClients_, and updatableStateElements_.

Referenced by pollingLoop().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ postEvent()

template<typename EventType >

void smacc2::SignalDetector::postEvent ( EventType * ev )

inline

Definition at line 61 of file smacc_signal_detector.hpp.

  {
    boost::intrusive_ptr<EventType> weakPtrEvent = ev;
    this->scheduler_->queue_event(processorHandle_, weakPtrEvent);
  }

References processorHandle_, and scheduler_.

Referenced by smacc2::ISmaccStateMachine::postEvent().

Here is the caller graph for this function:

◆ runThread()

void smacc2::SignalDetector::runThread ( )

runThread()

Definition at line 229 of file signal_detector.cpp.

{
  signalDetectorThread_ = boost::thread(boost::bind(&SignalDetector::pollingLoop, this));
}

References pollingLoop(), and signalDetectorThread_.

Here is the call graph for this function:

◆ setProcessorHandle()

void smacc2::SignalDetector::setProcessorHandle ( SmaccFifoScheduler::processor_handle processorHandle )

setProcessorHandle()

Definition at line 219 of file signal_detector.cpp.

{
  processorHandle_ = processorHandle;
}

References processorHandle_.

Referenced by smacc2::run(), and smacc2::run_async().

Here is the caller graph for this function:

◆ stop()

void smacc2::SignalDetector::stop ( )

stop()

Definition at line 246 of file signal_detector.cpp.

246{ end_ = true; }

References end_.

Referenced by smacc2::onSignalShutdown().

Here is the caller graph for this function:

◆ terminateScheduler()

void smacc2::SignalDetector::terminateScheduler ( )

terminateScheduler()

Definition at line 253 of file signal_detector.cpp.

{
  if (scheduler_)
  {
    RCLCPP_INFO(getLogger(), "[SignalDetector] Terminating scheduler...");
    scheduler_->terminate();
  }
}

References getLogger(), and scheduler_.

Referenced by smacc2::run().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ end_

std::atomic<bool> smacc2::SignalDetector::end_

private

Definition at line 95 of file smacc_signal_detector.hpp.

Referenced by pollingLoop(), SignalDetector(), and stop().

◆ executionModel_

ExecutionModel smacc2::SignalDetector::executionModel_

private

Definition at line 112 of file smacc_signal_detector.hpp.

Referenced by pollingLoop(), and SignalDetector().

◆ initialized_

std::atomic<bool> smacc2::SignalDetector::initialized_

private

Definition at line 97 of file smacc_signal_detector.hpp.

Referenced by initialize(), pollingLoop(), and SignalDetector().

◆ lastState_

std::atomic<int64_t> smacc2::SignalDetector::lastState_

private

Definition at line 86 of file smacc_signal_detector.hpp.

Referenced by initialize().

◆ loop_rate_hz

double smacc2::SignalDetector::loop_rate_hz

private

Definition at line 93 of file smacc_signal_detector.hpp.

Referenced by initialize(), pollingLoop(), and SignalDetector().

◆ processorHandle_

SmaccFifoScheduler::processor_handle smacc2::SignalDetector::processorHandle_

private

Definition at line 108 of file smacc_signal_detector.hpp.

Referenced by postEvent(), and setProcessorHandle().

◆ rosInitialized_

std::atomic<bool> smacc2::SignalDetector::rosInitialized_

private

Definition at line 100 of file smacc_signal_detector.hpp.

Referenced by notifyRosInitialized(), pollingLoop(), and SignalDetector().

◆ scheduler_

SmaccFifoScheduler* smacc2::SignalDetector::scheduler_

private

Definition at line 106 of file smacc_signal_detector.hpp.

Referenced by postEvent(), SignalDetector(), and terminateScheduler().

◆ signalDetectorThread_

boost::thread smacc2::SignalDetector::signalDetectorThread_

private

Definition at line 110 of file smacc_signal_detector.hpp.

Referenced by join(), and runThread().

◆ smaccStateMachine_

ISmaccStateMachine* smacc2::SignalDetector::smaccStateMachine_

private

Definition at line 81 of file smacc_signal_detector.hpp.

Referenced by findUpdatableClientsAndComponents(), findUpdatableStateElements(), getNode(), initialize(), and pollOnce().

◆ statusPub_

rclcpp::Publisher<smacc2_msgs::msg::SmaccStatus>::SharedPtr smacc2::SignalDetector::statusPub_

private

Definition at line 102 of file smacc_signal_detector.hpp.

◆ updatableClients_

std::vector<ISmaccUpdatable *> smacc2::SignalDetector::updatableClients_

private

Definition at line 83 of file smacc_signal_detector.hpp.

Referenced by findUpdatableClientsAndComponents(), and pollOnce().

◆ updatableStateElements_

std::vector<std::vector<ISmaccUpdatable *> > smacc2::SignalDetector::updatableStateElements_

private

Definition at line 85 of file smacc_signal_detector.hpp.

Referenced by findUpdatableStateElements(), notifyStateExited(), and pollOnce().

The documentation for this class was generated from the following files:

smacc2/include/smacc2/smacc_signal_detector.hpp
smacc2/src/smacc2/signal_detector.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ SignalDetector()

Member Function Documentation

◆ findUpdatableClientsAndComponents()

◆ findUpdatableStateElements()

◆ getLogger()

◆ getNode()

◆ initialize()

◆ join()

◆ notifyRosInitialized()

◆ notifyStateConfigured()

◆ notifyStateExited()

◆ pollingLoop()

◆ pollOnce()

◆ postEvent()

◆ runThread()

◆ setProcessorHandle()

◆ stop()

◆ terminateScheduler()

Member Data Documentation

◆ end_

◆ executionModel_

◆ initialized_

◆ lastState_

◆ loop_rate_hz

◆ processorHandle_

◆ rosInitialized_

◆ scheduler_

◆ signalDetectorThread_

◆ smaccStateMachine_

◆ statusPub_

◆ updatableClients_

◆ updatableStateElements_