Program Listing for File vector_fsm.hpp

↰ Return to documentation for file (cosm/spatial/fsm/vector_fsm.hpp)

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "rcppsw/math/vector2.hpp"

#include "cosm/cosm.hpp"
#include "cosm/spatial/fsm/util_hfsm.hpp"
#include "cosm/ta/taskable.hpp"
#include "cosm/spatial/fsm/point_argument.hpp"

/*******************************************************************************
 * Namespaces
 ******************************************************************************/
namespace cosm::spatial::fsm {

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class vector_fsm final : public csfsm::util_hfsm,
                         public rer::client<vector_fsm>,
                         public cta::taskable {
 public:
  vector_fsm(const csfsm::fsm_params* params, rmath::rng* rng);

  vector_fsm& operator=(const vector_fsm&) = delete;
  vector_fsm(const vector_fsm&) = delete;

  /* taskable overrides */
  void task_reset(void) override;
  bool task_running(void) const override {
    return current_state() != ekST_START && current_state() != ekST_ARRIVED;
  }
  void task_execute(void) override;
  void task_start(ta::taskable_argument* c_arg) override;
  bool task_finished(void) const override {
    return current_state() == ekST_ARRIVED;
  }

  const rmath::vector2d& target(void) const { return m_goal.point(); }

  /* HFSM overrides */
  void init(void) override;

  /* interference metrics */
  bool exp_interference(void) const override RCPPSW_PURE;
  bool entered_interference(void) const override RCPPSW_PURE;
  bool exited_interference(void) const override RCPPSW_PURE;
  boost::optional<rmath::vector3z> interference_loc3D(void) const override RCPPSW_PURE;

 private:
  enum state {
    ekST_START,
    ekST_VECTOR,

    ekST_INTERFERENCE_AVOIDANCE,

    ekST_INTERFERENCE_RECOVERY,

    ekST_ARRIVED,
    ekST_MAX_STATES
  };

  struct fsm_state {
    size_t m_interference_rec_count{0};
  };

  static constexpr const size_t kINTERFERENCE_RECOVERY_TIME = 10;

  rmath::vector2d calc_vector_to_goal(const rmath::vector2d& goal) RCPPSW_PURE;

  /* vector states */
  RCPPSW_HFSM_STATE_DECLARE_ND(vector_fsm, start);
  RCPPSW_HFSM_STATE_DECLARE_ND(vector_fsm, vector);
  RCPPSW_HFSM_STATE_DECLARE_ND(vector_fsm, interference_avoidance);
  RCPPSW_HFSM_STATE_DECLARE_ND(vector_fsm, interference_recovery);
  RCPPSW_HFSM_STATE_DECLARE(vector_fsm, arrived, point_argument);

  RCPPSW_HFSM_ENTRY_DECLARE_ND(vector_fsm, entry_vector);
  RCPPSW_HFSM_ENTRY_DECLARE_ND(vector_fsm, entry_interference_avoidance);
  RCPPSW_HFSM_ENTRY_DECLARE_ND(vector_fsm, entry_interference_recovery);

  RCPPSW_HFSM_EXIT_DECLARE(vector_fsm, exit_interference_avoidance);

  RCPPSW_HFSM_DEFINE_STATE_MAP_ACCESSOR(state_map_ex, index) override {
    return &mc_state_map[index];
  }

  RCPPSW_HFSM_DECLARE_STATE_MAP(state_map_ex, mc_state_map, ekST_MAX_STATES);

  /* clang-format off */
  fsm_state      m_state{};
  point_argument m_goal{};
  /* clang-format on */
};

} /* namespace cosm::spatial::fsm */