smacc::SignalDetector Class Reference

#include <smacc_signal_detector.h>

Collaboration diagram for smacc::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	pollingLoop ()
template<typename EventType >
void	postEvent (EventType *ev)
void	notifyStateConfigured (ISmaccState *currentState)
void	notifyStateExited (ISmaccState *currentState)

Private Member Functions
void	pollOnce ()
void	findUpdatableClients ()
void	findUpdatableStateElements (ISmaccState *currentState)

Private Attributes
ISmaccStateMachine *	smaccStateMachine_
std::vector< ISmaccUpdatable * >	updatableClients_
std::vector< std::vector< ISmaccUpdatable * > >	updatableStateElements_
std::atomic< uint64_t >	lastState_
double	loop_rate_hz
std::atomic< bool >	end_
std::atomic< bool >	initialized_
ros::NodeHandle	nh_
ros::Publisher	statusPub_
SmaccFifoScheduler *	scheduler_
SmaccFifoScheduler::processor_handle	processorHandle_
boost::thread	signalDetectorThread_
ExecutionModel	executionModel_

Detailed Description

Definition at line 22 of file smacc_signal_detector.h.

Constructor & Destructor Documentation

SignalDetector()

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

SignalDetector()

Definition at line 18 of file signal_detector.cpp.

  : end_(false),
    initialized_(false),
    executionModel_(executionModel)
{
  scheduler_ = scheduler;
  loop_rate_hz = 20.0;
}

References loop_rate_hz, and scheduler_.

Member Function Documentation

findUpdatableClients()

void smacc::SignalDetector::findUpdatableClients ( )

private

findUpdatableClients()

Definition at line 45 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)
      {
        ROS_DEBUG_STREAM("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)
        {
          ROS_DEBUG_STREAM("Adding updatable component: " << demangleType(typeid(*updatableComponent)));
          this->updatableClients_.push_back(updatableComponent);
        }
      }
    }
  }
}

References smacc::introspection::demangleType(), smacc::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 smacc::SignalDetector::findUpdatableStateElements ( ISmaccState *  currentState )

private

findUpdatableClientBehaviors()

Definition at line 85 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)
      {
        ROS_DEBUG_STREAM("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)
    {
      ROS_DEBUG_STREAM("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)
    {
      ROS_DEBUG_STREAM("Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)));
      updatableElements.push_back(updatableEventGenerator);
    }
  }
}

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

Referenced by notifyStateConfigured().

Here is the call graph for this function:

Here is the caller graph for this function:

initialize()

void smacc::SignalDetector::initialize

(

ISmaccStateMachine *

stateMachine

)

initialize()

Definition at line 32 of file signal_detector.cpp.

{
  smaccStateMachine_ = stateMachine;
  lastState_ = std::numeric_limits<uint64_t>::quiet_NaN();
  findUpdatableClients();
  initialized_ = true;
}

References findUpdatableClients(), initialized_, lastState_, and smaccStateMachine_.

Referenced by smacc::ISmaccStateMachine::ISmaccStateMachine().

Here is the call graph for this function:

Here is the caller graph for this function:

join()

void smacc::SignalDetector::join

(

)

join()

Definition at line 162 of file signal_detector.cpp.

{
  signalDetectorThread_.join();
}

References signalDetectorThread_.

notifyStateConfigured()

void smacc::SignalDetector::notifyStateConfigured

(

ISmaccState *

currentState

)

Definition at line 177 of file signal_detector.cpp.

{
  this->findUpdatableStateElements(currentState);
}

References findUpdatableStateElements().

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

Here is the call graph for this function:

Here is the caller graph for this function:

notifyStateExited()

void smacc::SignalDetector::notifyStateExited

(

ISmaccState *

currentState

)

Definition at line 182 of file signal_detector.cpp.

{
  this->updatableStateElements_.pop_back();
}

References updatableStateElements_.

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

Here is the caller graph for this function:

pollingLoop()

void smacc::SignalDetector::pollingLoop

(

)

pollingLoop()

Definition at line 267 of file signal_detector.cpp.

{
  // ros::NodeHandle nh("~"); // use node name as root of the parameter server
 
  ros::NodeHandle _;
  ros::Rate r0(20);
  while (!initialized_)
  {
    r0.sleep();
  }
 
  ros::NodeHandle nh(smacc::utils::cleanShortTypeName(typeid(*this->smaccStateMachine_)));
 
  if (!nh.getParam("signal_detector_loop_freq", this->loop_rate_hz))
  {
    ROS_WARN("Signal detector frequency (ros param ~signal_detector_loop_freq) was not set, using default frequency: "
             "%lf",
             this->loop_rate_hz);
  }
  else
  {
    ROS_WARN("Signal detector frequency (ros param ~signal_detector_loop_freq): %lf", this->loop_rate_hz);
  }
 
  nh.setParam("signal_detector_loop_freq", this->loop_rate_hz);
 
  ROS_INFO_STREAM("[SignalDetector] loop rate hz:" << loop_rate_hz);
 
  if (this->executionModel_ == ExecutionModel::SINGLE_THREAD_SPINNER)
  {
    ROS_INFO_STREAM("[SignalDetector] running in single threaded mode");
 
    ros::Rate r(loop_rate_hz);
    while (ros::ok() && !end_)
    {
      ROS_INFO_STREAM_THROTTLE(10, "[SignalDetector] heartbeat");
      pollOnce();
      ros::spinOnce();
      r.sleep();
    }
  }
  else
  {
    ROS_INFO_STREAM("[SignalDetector] running in multi threaded mode");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::waitForShutdown();
  }
 
}

References smacc::utils::cleanShortTypeName(), end_, executionModel_, initialized_, loop_rate_hz, pollOnce(), smacc::SINGLE_THREAD_SPINNER, and smaccStateMachine_.

Referenced by smacc::run(), and runThread().

Here is the call graph for this function:

Here is the caller graph for this function:

pollOnce()

void smacc::SignalDetector::pollOnce ( )

private

poll()

Definition at line 193 of file signal_detector.cpp.

{
  // precondition: smaccStateMachine_ != nullptr
 
  try
  {
    smaccStateMachine_->lockStateMachine("update behaviors");
 
    this->findUpdatableClients();
    ROS_DEBUG_STREAM("updatable clients: " << this->updatableClients_.size());
 
    if (this->updatableClients_.size())
    {
      for (auto *updatableClient : this->updatableClients_)
      {
        ROS_DEBUG_STREAM("[PollOnce] update client call:  " << demangleType(typeid(updatableClient)));
        updatableClient->executeUpdate();
      }
    }
 
    // STATE UPDATABLE ELEMENTS
    if (this->smaccStateMachine_->stateMachineCurrentAction != StateMachineInternalAction::TRANSITIONING &&
        this->smaccStateMachine_->stateMachineCurrentAction != StateMachineInternalAction::STATE_CONFIGURING &&
        this->smaccStateMachine_->stateMachineCurrentAction != StateMachineInternalAction::STATE_EXITING &&
        this->smaccStateMachine_->stateMachineCurrentAction != StateMachineInternalAction::STATE_ENTERING)
    {
 
      // // we do not update updatable elements during trasitioning or configuration of states
      // long currentStateIndex = smaccStateMachine_->getCurrentStateCounter();
 
      // ROS_DEBUG_STREAM("[PollOnce] update behaviors. checking current state");
 
      // auto currentState = smaccStateMachine_->getCurrentState();
      // if (currentState != nullptr)
      // {
      //   ROS_DEBUG_STREAM("[PollOnce] current state: " << currentStateIndex);
      //   ROS_DEBUG_STREAM("[PollOnce] last state: " << this->lastState_);
 
      //   if (currentStateIndex != 0)
      //   {
      //     if (currentStateIndex != this->lastState_)
      //     {
      //       ROS_DEBUG_STREAM("[PollOnce] detected new state, refreshing updatable client behavior table");
      //       // we are in a new state, refresh the updatable client behaviors table
      //       this->lastState_ = currentStateIndex;
      //     }
 
        ROS_DEBUG_STREAM("updatable states: " << this->updatableStateElements_.size());
 
        for (auto stateElement : this->updatableStateElements_)
        {
          for (auto *udpatableStateElement : stateElement)
          {
            ROS_DEBUG_STREAM("pollOnce update client behavior call: " << demangleType(typeid(*udpatableStateElement)));
            udpatableStateElement->executeUpdate();
          }
        }
        // }
      //}
    }
  }
  catch (std::exception &ex)
  {
    ROS_ERROR("Exception during Signal Detector update loop. %s", ex.what());
  }
 
  smaccStateMachine_->unlockStateMachine("update behaviors");
}

References smacc::introspection::demangleType(), findUpdatableClients(), smacc::ISmaccStateMachine::lockStateMachine(), smaccStateMachine_, smacc::STATE_CONFIGURING, smacc::STATE_ENTERING, smacc::STATE_EXITING, smacc::ISmaccStateMachine::stateMachineCurrentAction, smacc::TRANSITIONING, smacc::ISmaccStateMachine::unlockStateMachine(), 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 smacc::SignalDetector::postEvent ( EventType *  ev )

inline

Definition at line 42 of file smacc_signal_detector.h.

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

References processorHandle_, and scheduler_.

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

Here is the caller graph for this function:

runThread()

void smacc::SignalDetector::runThread

(

)

runThread()

Definition at line 152 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 smacc::SignalDetector::setProcessorHandle

(

SmaccFifoScheduler::processor_handle

processorHandle

)

setProcessorHandle()

Definition at line 142 of file signal_detector.cpp.

{
  processorHandle_ = processorHandle;
}

References processorHandle_.

Referenced by smacc::run().

Here is the caller graph for this function:

stop()

void smacc::SignalDetector::stop

(

)

stop()

Definition at line 172 of file signal_detector.cpp.

{
  end_ = true;
}

References end_.

Member Data Documentation

end_

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

private

Definition at line 69 of file smacc_signal_detector.h.

Referenced by pollingLoop(), and stop().

executionModel_

ExecutionModel smacc::SignalDetector::executionModel_

private

Definition at line 85 of file smacc_signal_detector.h.

Referenced by pollingLoop().

initialized_

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

private

Definition at line 71 of file smacc_signal_detector.h.

Referenced by initialize(), and pollingLoop().

lastState_

std::atomic<uint64_t> smacc::SignalDetector::lastState_

private

Definition at line 61 of file smacc_signal_detector.h.

Referenced by initialize().

loop_rate_hz

double smacc::SignalDetector::loop_rate_hz

private

Definition at line 67 of file smacc_signal_detector.h.

Referenced by pollingLoop(), and SignalDetector().

nh_

ros::NodeHandle smacc::SignalDetector::nh_

private

Definition at line 73 of file smacc_signal_detector.h.

processorHandle_

SmaccFifoScheduler::processor_handle smacc::SignalDetector::processorHandle_

private

Definition at line 81 of file smacc_signal_detector.h.

Referenced by postEvent(), and setProcessorHandle().

scheduler_

SmaccFifoScheduler* smacc::SignalDetector::scheduler_

private

Definition at line 79 of file smacc_signal_detector.h.

Referenced by postEvent(), and SignalDetector().

signalDetectorThread_

boost::thread smacc::SignalDetector::signalDetectorThread_

private

Definition at line 83 of file smacc_signal_detector.h.

Referenced by join(), and runThread().

smaccStateMachine_

ISmaccStateMachine* smacc::SignalDetector::smaccStateMachine_

private

Definition at line 55 of file smacc_signal_detector.h.

Referenced by findUpdatableClients(), findUpdatableStateElements(), initialize(), pollingLoop(), and pollOnce().

statusPub_

ros::Publisher smacc::SignalDetector::statusPub_

private

Definition at line 75 of file smacc_signal_detector.h.

updatableClients_

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

private

Definition at line 57 of file smacc_signal_detector.h.

Referenced by findUpdatableClients(), and pollOnce().

updatableStateElements_

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

private

Definition at line 59 of file smacc_signal_detector.h.

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

---

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

• smacc/include/smacc/smacc_signal_detector.h
• smacc/src/smacc/signal_detector.cpp