smacc_state_machine.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 <boost/any.hpp>
#include <map>
#include <mutex>
 
#include <smacc2/common.hpp>
#include <smacc2/introspection/introspection.hpp>
#include <smacc2/introspection/smacc_state_machine_info.hpp>
#include <smacc2/smacc_signal.hpp>
#include <smacc2/smacc_updatable.hpp>
 
#include <smacc2_msgs/msg/smacc_state_machine.hpp>
#include <smacc2_msgs/msg/smacc_status.hpp>
#include <smacc2_msgs/msg/smacc_transition_log_entry.hpp>
#include <smacc2_msgs/srv/smacc_get_transition_history.hpp>
 
#include <smacc2/callback_counter_semaphore.hpp>
#include <smacc2/smacc_state.hpp>
#include <smacc2/smacc_state_reactor.hpp>
//#include <smacc2/smacc_event_generator.hpp>
 
namespace smacc2
{
using namespace smacc2::introspection;
 
enum class EventLifeTime
{
  ABSOLUTE,
  CURRENT_STATE /*events are discarded if we are leaving the state it were created. I is used for client behaviors whose lifetime is associated to state*/
};
 
enum class StateMachineInternalAction
{
  STATE_CONFIGURING,
  STATE_ENTERING,
  STATE_RUNNING,
  STATE_EXITING,
  TRANSITIONING
};
 
// This class describes the concept of Smacc State Machine in an abastract way.
// The SmaccStateMachineBase inherits from this state machine and from
// statechart::StateMachine<> (via multiple inheritance)
class ISmaccStateMachine
{
public:
  ISmaccStateMachine(
    std::string stateMachineName, SignalDetector * signalDetector,
    rclcpp::NodeOptions nodeOptions = rclcpp::NodeOptions());
 
  virtual ~ISmaccStateMachine();
 
  virtual void reset();
 
  virtual void stop();
 
  virtual void eStop();
 
  template <typename TOrthogonal>
  TOrthogonal * getOrthogonal();
 
  // gets the client behavior in a given orthogonal
  // the index is used to distinguish between multiple client behaviors of the same type
  template <typename TOrthogonal, typename TClientBehavior>
  inline TClientBehavior * getClientBehavior(int index = 0)
  {
    auto orthogonal = this->template getOrthogonal<TOrthogonal>();
 
    return orthogonal->template getClientBehavior<TClientBehavior>(index);
  }
 
  const std::map<std::string, std::shared_ptr<smacc2::ISmaccOrthogonal>> & getOrthogonals() const;
 
  template <typename SmaccComponentType>
  void requiresComponent(SmaccComponentType *& storage, bool throwsExceptionIfNotExist = false);
 
  template <typename EventType>
  void postEvent(EventType * ev, EventLifeTime evlifetime = EventLifeTime::ABSOLUTE);
 
  template <typename EventType>
  void postEvent(EventLifeTime evlifetime = EventLifeTime::ABSOLUTE);
 
  // gets data from the state machine blackboard
  template <typename T>
  bool getGlobalSMData(std::string name, T & ret);
 
  // sets data in the state machine blackboard
  template <typename T>
  void setGlobalSMData(std::string name, T value);
 
  template <typename StateField, typename BehaviorType>
  void mapBehavior();
 
  std::string getStateMachineName();
 
  void state_machine_visualization();
 
  inline std::shared_ptr<SmaccStateInfo> getCurrentStateInfo() { return currentStateInfo_; }
 
  void publishTransition(const SmaccTransitionInfo & transitionInfo);
 
  virtual void onInitialize();
 
  void getTransitionLogHistory(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Request> req,
    std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Response> res);
 
  template <typename TSmaccSignal, typename TMemberFunctionPrototype, typename TSmaccObjectType>
  boost::signals2::connection createSignalConnection(
    TSmaccSignal & signal, TMemberFunctionPrototype callback, TSmaccObjectType * object);
 
  void disconnectSmaccSignalObject(void * object);
 
  template <typename StateType>
  void notifyOnStateEntryStart(StateType * state);
 
  template <typename StateType>
  void notifyOnStateEntryEnd(StateType * state);
 
  template <typename StateType>
  void notifyOnRuntimeConfigured(StateType * state);
 
  template <typename StateType>
  void notifyOnStateExitting(StateType * state);
 
  template <typename StateType>
  void notifyOnStateExited(StateType * state);
 
  template <typename StateType>
  void notifyOnRuntimeConfigurationFinished(StateType * state);
 
  inline int64_t getCurrentStateCounter() const;
 
  inline ISmaccState * getCurrentState() const;
 
  inline const SmaccStateMachineInfo & getStateMachineInfo();
 
  template <typename InitialStateType>
  void buildStateMachineInfo();
 
  rclcpp::Node::SharedPtr getNode();
 
  inline rclcpp::Logger getLogger() { return nh_->get_logger(); }
 
  inline std::recursive_mutex & getMutex() { return this->m_mutex_; }
 
protected:
  void checkStateMachineConsistence();
 
  void initializeROS(std::string smshortname);
 
  void onInitialized();
 
  template <typename TOrthogonal>
  void createOrthogonal();
 
  // The node handle for this state
  rclcpp::Node::SharedPtr nh_;
 
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<smacc2_msgs::msg::SmaccStateMachine>::SharedPtr stateMachinePub_;
  rclcpp::Publisher<smacc2_msgs::msg::SmaccStatus>::SharedPtr stateMachineStatusPub_;
  rclcpp::Publisher<smacc2_msgs::msg::SmaccTransitionLogEntry>::SharedPtr transitionLogPub_;
  rclcpp::Service<smacc2_msgs::srv::SmaccGetTransitionHistory>::SharedPtr transitionHistoryService_;
 
  // if it is null, you may be located in a transition. There is a small gap of time where internally
  // this currentState_ is null. This may change in the future.
  std::vector<ISmaccState *> currentState_;
 
  std::shared_ptr<SmaccStateInfo> currentStateInfo_;
 
  smacc2_msgs::msg::SmaccStatus status_msg_;
 
  // orthogonals
  std::map<std::string, std::shared_ptr<smacc2::ISmaccOrthogonal>> orthogonals_;
 
  std::vector<boost::signals2::scoped_connection> longLivedSignalConnections_;
 
protected:
  std::shared_ptr<SmaccStateMachineInfo> stateMachineInfo_;
 
private:
  std::recursive_mutex m_mutex_;
  std::recursive_mutex eventQueueMutex_;
 
  StateMachineInternalAction stateMachineCurrentAction;
 
  std::map<void *, std::shared_ptr<CallbackCounterSemaphore>> stateCallbackConnections;
 
  // shared variables
  std::map<std::string, std::pair<std::function<std::string()>, boost::any>> globalData_;
 
  // contains the whole history of transitions of the state machine
  std::vector<smacc2_msgs::msg::SmaccTransitionLogEntry> transitionLogHistory_;
 
  smacc2::SMRunMode runMode_;
 
  // Event to notify to the signaldetection thread that a request has been created...
  SignalDetector * signalDetector_;
 
  int64_t stateSeqCounter_;
 
  void lockStateMachine(std::string msg);
 
  void unlockStateMachine(std::string msg);
 
  template <typename EventType>
  void propagateEventToStateReactors(ISmaccState * st, EventType * ev);
 
  void updateStatusMessage();
 
  friend class ISmaccState;
  friend class SignalDetector;
};
}  // namespace smacc2
 
#include <smacc2/impl/smacc_client_impl.hpp>
#include <smacc2/impl/smacc_component_impl.hpp>
#include <smacc2/impl/smacc_orthogonal_impl.hpp>
#include <smacc2/impl/smacc_state_impl.hpp>