visual_servoing_state_machine.h

Go to the documentation of this file.

#pragma once

// back-end
#include <aerial_autonomy/common/thread_safe_state_machine.h>

// front-end
#include <boost/msm/front/state_machine_def.hpp>

// functors
#include <boost/msm/front/functor_row.hpp>

// Actions and guards used
#include <aerial_autonomy/actions_guards/visual_servoing_states_actions.h>

// Robot System used
#include <aerial_autonomy/robot_systems/uav_vision_system.h>

// Logging library
#include <glog/logging.h>

// Store state machine states
#include <array>

// Base state machine
#include <aerial_autonomy/state_machines/base_state_machine.h>

// Events
#include <aerial_autonomy/visual_servoing_events.h>

#include <aerial_autonomy/types/manual_control_event.h>

namespace msmf = boost::msm::front;
namespace vse = visual_servoing_events;
namespace be = uav_basic_events;

// Forward Declaration
struct VisualServoingStateMachineFrontEnd;

using VisualServoingStateMachine = boost::msm::back::thread_safe_state_machine<
    VisualServoingStateMachineFrontEnd>;

using vsa = VisualServoingStatesActions<VisualServoingStateMachine>;

class VisualServoingStateMachineFrontEnd
    : public msmf::state_machine_def<VisualServoingStateMachineFrontEnd>,
      public BaseStateMachine<UAVVisionSystem> {
public:
  template <class Event, class FSM> void on_entry(Event const &, FSM &) {
    VLOG(1) << "entering: VisualServoing system";
  }
  template <class Event, class FSM> void on_exit(Event const &, FSM &) {
    VLOG(1) << "leaving: VisualServoing system";
  }

  VisualServoingStateMachineFrontEnd(UAVVisionSystem &uav_system)
      : BaseStateMachine(uav_system) {}

  using initial_state = vsa::ManualControlState;
  struct transition_table
      : boost::mpl::vector<
            //        Start          Event         Next           Action Guard
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Landed, be::Takeoff, vsa::TakingOff,
                      vsa::TakeoffAction, vsa::TakeoffGuard>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Landed, ManualControlEvent, vsa::ManualControlState,
                      msmf::none, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::TakingOff, Completed, vsa::Hovering, msmf::none,
                      msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Hovering, PositionYaw, vsa::ReachingGoal,
                      vsa::ReachingGoalSet, vsa::ReachingGoalGuard>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Hovering, vse::TrackROI, vsa::VisualServoing,
                      vsa::VisualServoingTransitionAction,
                      vsa::VisualServoingTransitionGuard>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Hovering, vse::GoHome, vsa::ReachingGoal,
                      vsa::GoHomeTransitionAction, vsa::GoHomeTransitionGuard>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Hovering, be::Land, vsa::Landing, vsa::LandingAction,
                      msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Hovering, ManualControlEvent,
                      vsa::ManualControlState, msmf::none, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::ReachingGoal, be::Abort, vsa::Hovering,
                      vsa::UAVControllerAbort, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::ReachingGoal, be::Land, vsa::Landing,
                      vsa::ReachingGoalLand, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::VisualServoing, be::Abort, vsa::Hovering,
                      vsa::UAVControllerAbort, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::VisualServoing, be::Land, vsa::Landing,
                      vsa::ReachingGoalLand, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::Landing, Completed, vsa::Landed, msmf::none,
                      msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::ReachingGoal, Completed, vsa::Hovering,
                      vsa::UAVControllerAbort, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::VisualServoing, Completed, vsa::Hovering,
                      vsa::UAVControllerAbort, msmf::none>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::ManualControlState, be::Takeoff, vsa::Hovering,
                      vsa::ManualControlSwitchAction,
                      vsa::ManualControlSwitchGuard>,
            //        +--------------+-------------+--------------+---------------------+---------------------------+
            msmf::Row<vsa::ManualControlState, be::Land, vsa::Landed,
                      vsa::ManualControlSwitchAction,
                      vsa::ManualControlSwitchGuard>> {};
  using BaseStateMachine<UAVVisionSystem>::no_transition;
};

static constexpr std::array<const char *, 7> state_names = {
    "Landed",         "TakingOff", "Hovering",          "ReachingGoal",
    "VisualServoing", "Landing",   "ManualControlState"};
const char *pstate(VisualServoingStateMachine const &p) {
  return state_names.at(p.current_state()[0]);
}