smacc_signal_detector.hpp

Go to the documentation of this file.

// 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 <atomic>
#include <boost/thread.hpp>
#include <smacc2/common.hpp>
#include <smacc2_msgs/msg/smacc_status.hpp>
 
namespace smacc2
{
enum class ExecutionModel
{
  SINGLE_THREAD_SPINNER,
  MULTI_THREAD_SPINNER
};
 
// Mostly define the state machine ros thread and receive events state machine components (clients, state elements)
// This class also contains the event queue of the state machine
class SignalDetector
{
public:
  SignalDetector(
    SmaccFifoScheduler * scheduler,
    ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER);
 
  void initialize(ISmaccStateMachine * stateMachine);
 
  void setProcessorHandle(SmaccFifoScheduler::processor_handle processorHandle);
 
  // Runs the polling loop into a thread...
  void runThread();
 
  // Waits for the polling thread to end...
  void join();
 
  void stop();
 
  void terminateScheduler();
 
  void pollingLoop();
 
  template <typename EventType>
  void postEvent(EventType * ev)
  {
    boost::intrusive_ptr<EventType> weakPtrEvent = ev;
    this->scheduler_->queue_event(processorHandle_, weakPtrEvent);
  }
 
  rclcpp::Node::SharedPtr getNode();
  inline rclcpp::Logger getLogger() { return getNode()->get_logger(); }
 
  void notifyStateConfigured(ISmaccState * currentState);
 
  void notifyStateExited(ISmaccState * currentState);
 
  // Called after state machine ROS initialization is complete (initializeROS)
  // This signals that orthogonals, clients, and ROS objects are ready
  void notifyRosInitialized();
 
private:
  void pollOnce();
 
  ISmaccStateMachine * smaccStateMachine_;
 
  std::vector<ISmaccUpdatable *> updatableClients_;
 
  std::vector<std::vector<ISmaccUpdatable *>> updatableStateElements_;
  std::atomic<int64_t> lastState_;
 
  void findUpdatableClientsAndComponents();
 
  void findUpdatableStateElements(ISmaccState * currentState);
 
  // Loop frequency of the signal detector (to check answers from actionservers)
  double loop_rate_hz;
 
  std::atomic<bool> end_;
 
  std::atomic<bool> initialized_;
 
  // Flag set after initializeROS() completes - indicates ROS objects are ready
  std::atomic<bool> rosInitialized_;
 
  rclcpp::Publisher<smacc2_msgs::msg::SmaccStatus>::SharedPtr statusPub_;
 
  // ---- boost statechart related ----
 
  SmaccFifoScheduler * scheduler_;
 
  SmaccFifoScheduler::processor_handle processorHandle_;
 
  boost::thread signalDetectorThread_;
 
  ExecutionModel executionModel_;
};
 
void onSigQuit(int sig);
 
// Signal handler for graceful shutdown (SIGINT/SIGTERM)
void onSignalShutdown(int sig);
 
// Singleton class managing SMACC execution lifecycle
// Replaces global g_signal_detector for signal handler access
class SmExecution
{
public:
  // Get singleton instance
  static SmExecution & getInstance();
 
  // Delete copy and move constructors/operators
  SmExecution(const SmExecution &) = delete;
  SmExecution & operator=(const SmExecution &) = delete;
  SmExecution(SmExecution &&) = delete;
  SmExecution & operator=(SmExecution &&) = delete;
 
  boost::thread * schedulerThread;
  boost::thread * signalDetectorLoop;
  SignalDetector * signalDetector;
  SmaccFifoScheduler * scheduler1;
  SmaccFifoScheduler::processor_handle sm;
 
private:
  // Private constructor for singleton
  SmExecution();
};
 
// Main entry point for any SMACC state machine
// It instantiates and starts the specified state machine type
// it uses two threads: a new thread and the current one.
// The created thread is for the state machine process
// it locks the current thread to handle events of the state machine
template <typename StateMachineType>
void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER)
{
  // Register signal handlers for graceful shutdown
  ::signal(SIGINT, onSignalShutdown);
  ::signal(SIGTERM, onSignalShutdown);
  ::signal(SIGQUIT, onSigQuit);
 
  // Get singleton instance
  SmExecution & smExecution = SmExecution::getInstance();
 
  // create the asynchronous state machine scheduler
  SmaccFifoScheduler scheduler1(true);
 
  // create the signalDetector component
  SignalDetector signalDetector(&scheduler1, executionModel);
 
  // Store in singleton for signal handler access
  smExecution.signalDetector = &signalDetector;
  smExecution.scheduler1 = &scheduler1;
 
  // create the asynchronous state machine processor
  SmaccFifoScheduler::processor_handle sm =
    scheduler1.create_processor<StateMachineType>(&signalDetector);
 
  // initialize the asynchronous state machine processor
  signalDetector.setProcessorHandle(sm);
  smExecution.sm = sm;
 
  scheduler1.initiate_processor(sm);
 
  //create a thread for the asynchronous state machine processor execution
  boost::thread schedulerThread(boost::bind(&sc::fifo_scheduler<>::operator(), &scheduler1, 0));
  smExecution.schedulerThread = &schedulerThread;
 
  // use the  main thread for the signal detector component (waiting actionclient requests)
  signalDetector.pollingLoop();
 
  // After polling loop exits (due to shutdown), terminate the scheduler from main thread
  // This is safe to call here (not from signal handler) as we're in the main thread context
  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Polling loop exited. Terminating scheduler...");
  signalDetector.terminateScheduler();
 
  // Wait for scheduler thread to finish
  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Waiting for scheduler thread to join...");
  schedulerThread.join();
  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Scheduler thread terminated. Shutdown complete.");
 
  // Clear singleton references
  smExecution.signalDetector = nullptr;
  smExecution.scheduler1 = nullptr;
  smExecution.schedulerThread = nullptr;
}
 
template <typename StateMachineType>
SmExecution & run_async()
{
  // Register signal handlers for graceful shutdown
  ::signal(SIGINT, onSignalShutdown);
  ::signal(SIGTERM, onSignalShutdown);
  ::signal(SIGQUIT, onSigQuit);
 
  // Get singleton instance
  SmExecution & smExecution = SmExecution::getInstance();
 
  // create the asynchronous state machine scheduler
  smExecution.scheduler1 = new SmaccFifoScheduler(true);
 
  // create the signalDetector component
  smExecution.signalDetector = new SignalDetector(smExecution.scheduler1);
 
  // create the asynchronous state machine processor
  smExecution.sm =
    smExecution.scheduler1->create_processor<StateMachineType>(smExecution.signalDetector);
 
  // initialize the asynchronous state machine processor
  smExecution.signalDetector->setProcessorHandle(smExecution.sm);
 
  smExecution.scheduler1->initiate_processor(smExecution.sm);
 
  //create a thread for the asynchronous state machine processor execution
  smExecution.schedulerThread =
    new boost::thread(boost::bind(&sc::fifo_scheduler<>::operator(), smExecution.scheduler1, NULL));
  smExecution.signalDetectorLoop =
    new boost::thread(boost::bind(&SignalDetector::pollingLoop, smExecution.signalDetector));
 
  return smExecution;
}
 
}  // namespace smacc2