#include <smacc_signal_detector.hpp>

Collaboration diagram for smacc2::SignalDetector:

Public Member Functions
	SignalDetector (SmaccFifoScheduler *scheduler)

void	initialize (ISmaccStateMachine *stateMachine)

void	setProcessorHandle (SmaccFifoScheduler::processor_handle processorHandle)

void	runThread ()

void	join ()

void	stop ()

void	pollingLoop ()

void	pollOnce ()

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

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger ()

Private Member Functions
void	findUpdatableClientsAndComponents ()

void	findUpdatableStateElements (ISmaccState *currentState)

Private Attributes
ISmaccStateMachine *	smaccStateMachine_

std::vector< ISmaccUpdatable * >	updatableClients_

std::vector< ISmaccUpdatable * >	updatableStateElements_

std::atomic< unsigned long >	lastState_

double	loop_rate_hz

std::atomic< bool >	end_

std::atomic< bool >	initialized_

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

SmaccFifoScheduler *	scheduler_

SmaccFifoScheduler::processor_handle	processorHandle_

boost::thread	signalDetectorThread_

Detailed Description

Definition at line 29 of file smacc_signal_detector.hpp.

Constructor & Destructor Documentation

◆ SignalDetector()

smacc2::SignalDetector::SignalDetector ( SmaccFifoScheduler * scheduler )

SignalDetector()

Definition at line 43 of file signal_detector.cpp.

{
  scheduler_ = scheduler;
  loop_rate_hz = 20.0;
  end_ = false;
  initialized_ = false;
}

References end_, initialized_, loop_rate_hz, and scheduler_.

Member Function Documentation

◆ findUpdatableClientsAndComponents()

void smacc2::SignalDetector::findUpdatableClientsAndComponents ( )

private

findUpdatableClientsAndComponents()

Definition at line 73 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 116 of file signal_detector.cpp.

{
  this->updatableStateElements_.clear();
  for (auto pair : this->smaccStateMachine_->getOrthogonals())
  {
    auto & orthogonal = pair.second;
    auto & behaviors = orthogonal->getClientBehaviors();
 
    for (auto & currentBehavior : behaviors)
    {
      ISmaccUpdatable * updatableClientBehavior =
        dynamic_cast<ISmaccUpdatable *>(currentBehavior.get());
 
      if (updatableClientBehavior != nullptr)
      {
        RCLCPP_DEBUG_STREAM(
          getLogger(),
          "Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)));
        this->updatableStateElements_.push_back(updatableClientBehavior);
      }
    }
  }
 
  auto updatableState = dynamic_cast<ISmaccUpdatable *>(currentState);
  if (updatableState != nullptr)
  {
    this->updatableStateElements_.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)));
      this->updatableStateElements_.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)));
      this->updatableStateElements_.push_back(updatableEventGenerator);
    }
  }
}

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

Referenced by pollOnce().

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 58 of file smacc_signal_detector.hpp.

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

smacc2::SignalDetector::getNode

rclcpp::Node::SharedPtr getNode()

Definition: signal_detector.cpp:51

References getNode().

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

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 51 of file signal_detector.cpp.

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

smacc2::ISmaccStateMachine::getNode

rclcpp::Node::SharedPtr getNode()

Definition: smacc_state_machine.cpp:80

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 58 of file signal_detector.cpp.

{
  smaccStateMachine_ = stateMachine;
  lastState_ = std::numeric_limits<unsigned long>::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 197 of file signal_detector.cpp.

197{ signalDetectorThread_.join(); }

smacc2::SignalDetector::signalDetectorThread_

boost::thread signalDetectorThread_

Definition: smacc_signal_detector.hpp:87

References signalDetectorThread_.

◆ pollingLoop()

void smacc2::SignalDetector::pollingLoop ( )

pollingLoop()

Definition at line 336 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);
 
  while (!initialized_)
  {
    r0.sleep();
  }
 
  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);
 
  rclcpp::Rate r(loop_rate_hz);
 
  while (rclcpp::ok() && !end_)
  {
    pollOnce();
    rclcpp::spin_some(nh);
    r.sleep();
  }
}

References end_, getLogger(), getNode(), initialized_, loop_rate_hz, and pollOnce().

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ pollOnce()

void smacc2::SignalDetector::pollOnce ( )

poll()

Definition at line 211 of file signal_detector.cpp.

{
  // precondition: smaccStateMachine_ != nullptr
 
  //TRACEPOINT( spinOnce);
  TRACEPOINT(spinOnce);
 
  std::lock_guard<std::recursive_mutex> lock(smaccStateMachine_->m_mutex_);
  try
  {
    //smaccStateMachine_->lockStateMachine("update behaviors");
 
    long currentStateIndex = smaccStateMachine_->getCurrentStateCounter();
    auto currentState = smaccStateMachine_->getCurrentState();
 
    if (currentState != nullptr)
    {
      RCLCPP_INFO_THROTTLE(
        getLogger(), *(getNode()->get_clock()), 10000,
        "[SignalDetector] heartbeat. Current State: %s",
        demangleType(typeid(*currentState)).c_str());
    }
 
    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)));
 
          TRACEPOINT(smacc2_state_update_start, updatableElementName);
          updatableClient->executeUpdate(smaccStateMachine_->getNode());
          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_EXITING)
    {
      // we do not update updatable elements during trasitioning or configuration of states
      RCLCPP_DEBUG_STREAM(getLogger(), "[SignalDetector] update behaviors. checking current state");
 
      if (currentState != nullptr)
      {
        RCLCPP_DEBUG_STREAM(getLogger(), "[SignalDetector] current state: " << currentStateIndex);
        RCLCPP_DEBUG_STREAM(
          getLogger(), "[SignalDetector] last state: " << this->lastState_.load());
 
        if (currentStateIndex != 0)
        {
          if (currentStateIndex != (long)this->lastState_.load())
          {
            RCLCPP_DEBUG_STREAM(
              getLogger(),
              "[PollOnce] detected new state, refreshing updatable client "
              "behavior table");
            // we are in a new state, refresh the updatable client behaviors table
            this->lastState_.store(currentStateIndex);
            this->findUpdatableStateElements(currentState);
          }
 
          RCLCPP_DEBUG_STREAM(
            getLogger(),
            "[SignalDetector] updatable state elements: " << this->updatableStateElements_.size());
          auto node = getNode();
          for (auto * udpatableStateElement : this->updatableStateElements_)
          {
            auto updatableElementName = demangleType(typeid(*udpatableStateElement)).c_str();
            try
            {
              RCLCPP_DEBUG_STREAM(
                getLogger(),
                "[SignalDetector] update client behavior call: " << updatableElementName);
 
              TRACEPOINT(smacc2_state_update_start, updatableElementName);
              udpatableStateElement->executeUpdate(smaccStateMachine_->getNode());
              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');
            }
          }
        }
      }
    }
  }
  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(), findUpdatableStateElements(), smacc2::ISmaccStateMachine::getCurrentState(), smacc2::ISmaccStateMachine::getCurrentStateCounter(), getLogger(), getNode(), smacc2::ISmaccStateMachine::getNode(), lastState_, smacc2::ISmaccStateMachine::m_mutex_, smacc2_state_update_start, smaccStateMachine_, smacc2::STATE_CONFIGURING, smacc2::STATE_EXITING, smacc2::ISmaccStateMachine::stateMachineCurrentAction, TRACEPOINT(), 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 51 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 187 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 177 of file signal_detector.cpp.

{
  processorHandle_ = processorHandle;
}

References processorHandle_.

Referenced by smacc2::run().

Here is the caller graph for this function:

◆ stop()

void smacc2::SignalDetector::stop ( )

stop()

Definition at line 204 of file signal_detector.cpp.

204{ end_ = true; }

References end_.

Member Data Documentation

◆ end_

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

private

Definition at line 75 of file smacc_signal_detector.hpp.

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

◆ initialized_

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

private

Definition at line 77 of file smacc_signal_detector.hpp.

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

◆ lastState_

std::atomic<unsigned long> smacc2::SignalDetector::lastState_

private

Definition at line 67 of file smacc_signal_detector.hpp.

Referenced by initialize(), and pollOnce().

◆ loop_rate_hz

double smacc2::SignalDetector::loop_rate_hz

private

Definition at line 73 of file smacc_signal_detector.hpp.

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

◆ processorHandle_

SmaccFifoScheduler::processor_handle smacc2::SignalDetector::processorHandle_

private

Definition at line 85 of file smacc_signal_detector.hpp.

Referenced by postEvent(), and setProcessorHandle().

◆ scheduler_

SmaccFifoScheduler* smacc2::SignalDetector::scheduler_

private

Definition at line 83 of file smacc_signal_detector.hpp.

Referenced by postEvent(), and SignalDetector().

◆ signalDetectorThread_

boost::thread smacc2::SignalDetector::signalDetectorThread_

private

Definition at line 87 of file smacc_signal_detector.hpp.

Referenced by join(), and runThread().

◆ smaccStateMachine_

ISmaccStateMachine* smacc2::SignalDetector::smaccStateMachine_

private

Definition at line 61 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 79 of file smacc_signal_detector.hpp.

◆ updatableClients_

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

private

Definition at line 63 of file smacc_signal_detector.hpp.

Referenced by findUpdatableClientsAndComponents(), and pollOnce().

◆ updatableStateElements_

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

private

Definition at line 65 of file smacc_signal_detector.hpp.

Referenced by findUpdatableStateElements(), 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()

◆ pollingLoop()

◆ pollOnce()

◆ postEvent()

◆ runThread()

◆ setProcessorHandle()

◆ stop()

Member Data Documentation

◆ end_

◆ initialized_

◆ lastState_

◆ loop_rate_hz

◆ processorHandle_

◆ scheduler_

◆ signalDetectorThread_

◆ smaccStateMachine_

◆ statusPub_

◆ updatableClients_

◆ updatableStateElements_