Program Listing for File smacc_state_machine.hpp

↰ Return to documentation for file (/tmp/ws/src/smacc2/smacc2/include/smacc2/smacc_state_machine.hpp)

// 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>