Program Listing for File acquire_goal_fsm.hpp

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

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include <boost/optional.hpp>
#include <functional>
#include <list>
#include <memory>
#include <tuple>

#include "rcppsw/math/vector2.hpp"
#include "rcppsw/types/type_uuid.hpp"

#include "cosm/cosm.hpp"
#include "cosm/spatial/fsm/explore_for_goal_fsm.hpp"
#include "cosm/spatial/metrics/goal_acq_metrics.hpp"
#include "cosm/spatial/fsm/util_hfsm.hpp"
#include "cosm/spatial/fsm/vector_fsm.hpp"
#include "cosm/ta/taskable.hpp"

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

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class acquire_goal_fsm : public csfsm::util_hfsm,
                         public rer::client<acquire_goal_fsm>,
                         public cta::taskable,
                         public csmetrics::goal_acq_metrics {
 public:
  using candidate_type = std::tuple<rmath::vector2d, double, rtypes::type_uuid>;

  using goal_select_ftype = std::function<boost::optional<candidate_type>(void)>;

  using acquisition_goal_ftype =
      std::function<goal_acq_metrics::goal_type(void)>;

  using goal_valid_ftype =
      std::function<bool(const rmath::vector2d&, const rtypes::type_uuid&)>;

  struct hook_list {
    /* clang-format off */
    acquisition_goal_ftype    acquisition_goal{nullptr};

    goal_select_ftype         goal_select{nullptr};

    std::function<bool(void)> candidates_exist{nullptr};

    std::function<void(void)> begin_acq_cb{nullptr};

    std::function<bool(bool)> goal_acquired_cb{nullptr};

    std::function<bool(void)> explore_term_cb{nullptr};

     goal_valid_ftype goal_valid_cb{nullptr};
    /* clang-format on */
  };

  acquire_goal_fsm(const csfsm::fsm_params* params,
                   std::unique_ptr<cssexplore::base_explore> explore_behavior,
                   rmath::rng* rng,
                   const struct hook_list& hooks);
  ~acquire_goal_fsm(void) override = default;

  acquire_goal_fsm(const acquire_goal_fsm& fsm) = delete;
  acquire_goal_fsm& operator=(const acquire_goal_fsm& fsm) = delete;

  /* taskable overrides */
  void task_execute(void) override final;
  void task_start(ta::taskable_argument*) override {}
  bool task_finished(void) const override final {
    return ekST_FINISHED == current_state();
  }
  bool task_running(void) const override final {
    return ekST_ACQUIRE_GOAL == current_state();
  }
  void task_reset(void) override final { init(); }

  /* collision metrics */
  bool exp_interference(void) const override final RCPPSW_PURE;
  bool entered_interference(void) const override final RCPPSW_PURE;
  bool exited_interference(void) const override final RCPPSW_PURE;
  boost::optional<rtypes::timestep> interference_duration(void) const override final RCPPSW_PURE;
  boost::optional<rmath::vector3z> interference_loc3D(void) const override final RCPPSW_PURE;

  /* goal acquisition metrics */
  exp_status is_exploring_for_goal(void) const override final RCPPSW_PURE;
  bool is_vectoring_to_goal(void) const override final RCPPSW_PURE;
  bool goal_acquired(void) const override final RCPPSW_PURE;
  metrics::goal_acq_metrics::goal_type acquisition_goal(void) const override final;
  boost::optional<rmath::vector3z> acquisition_loc3D(void) const override final RCPPSW_PURE;
  boost::optional<rmath::vector3z> explore_loc3D(void) const override final RCPPSW_PURE;
  boost::optional<rmath::vector3z> vector_loc3D(void) const override final RCPPSW_PURE;
  rtypes::type_uuid entity_acquired_id(void) const override final {
    return m_acq_id;
  }

  void init(void) override final;

 protected:
  enum states {
    ekST_START,

    ekST_ACQUIRE_GOAL,

    ekST_FINISHED,
    ekST_MAX_STATES
  };

 private:
  bool acquire_goal(void);

  bool acquire_unknown_goal(void);

  bool acquire_known_goal(void);

  /* goal acquisition states */

  RCPPSW_HFSM_STATE_DECLARE_ND(acquire_goal_fsm, start);

  RCPPSW_HFSM_STATE_DECLARE_ND(acquire_goal_fsm, fsm_acquire_goal);

  RCPPSW_HFSM_STATE_DECLARE_ND(acquire_goal_fsm, finished);

  RCPPSW_HFSM_EXIT_DECLARE(acquire_goal_fsm, exit_fsm_acquire_goal);

  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 */
  rtypes::type_uuid              m_acq_id{rtypes::constants::kNoUUID};
  bool                           m_first_acq_step{false};
  hook_list                      m_hooks;
  vector_fsm                     m_vector_fsm;
  explore_for_goal_fsm           m_explore_fsm;
  /* clang-format on */
};

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