pick_place_functors.h

Go to the documentation of this file.

#pragma once
#include <aerial_autonomy/actions_guards/base_functors.h>
#include <aerial_autonomy/actions_guards/hovering_functors.h>
#include <aerial_autonomy/actions_guards/manual_control_functors.h>
#include <aerial_autonomy/actions_guards/shorting_action_sequence.h>
#include <aerial_autonomy/logic_states/base_state.h>
#include <aerial_autonomy/robot_systems/uav_arm_system.h>
#include <aerial_autonomy/types/completed_event.h>
#include <glog/logging.h>
#include <thread>

template <class LogicStateMachineT>
struct PickGuard_
    : EventAgnosticGuardFunctor<UAVArmSystem, LogicStateMachineT> {
  bool guard(UAVArmSystem &robot_system) {
    VLOG(1) << "Grip Object";
    bool success = robot_system.grip(true);
    // \todo this wait logic should be done on the arm plugin side and we should
    // be checking a status asynchronously for grip completion
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    if (!success) {
      LOG(WARNING) << "Failed to send grip command!";
      robot_system.grip(false);
      return false;
    }
    VLOG(1) << "Done Gripping!";
    return true;
  }
};

template <class LogicStateMachineT>
using PickInternalActionFunctor_ =
    boost::msm::front::ShortingActionSequence_<boost::mpl::vector<
        UAVStatusInternalActionFunctor_<LogicStateMachineT>,
        ArmStatusInternalActionFunctor_<LogicStateMachineT>,
        ControllerStatusInternalActionFunctor_<
            LogicStateMachineT, VisualServoingControllerArmConnector>,
        ControllerStatusInternalActionFunctor_<
            LogicStateMachineT, VisualServoingControllerDroneConnector,
            false>>>;
template <class LogicStateMachineT>
using ManualControlArmInternalActionFunctor_ =
    boost::msm::front::ShortingActionSequence_<boost::mpl::vector<
        ArmStatusInternalActionFunctor_<LogicStateMachineT>,
        ManualControlInternalActionFunctor_<LogicStateMachineT>>>;

template <class LogicStateMachineT>
struct PickTransitionGuardFunctor_
    : EventAgnosticGuardFunctor<UAVArmSystem, LogicStateMachineT> {
  bool guard(UAVArmSystem &robot_system_) {
    Position tracking_vector;
    return (robot_system_.getTrackingVector(tracking_vector) &&
            robot_system_.enabled());
  }
};

template <class LogicStateMachineT, int TransformIndex>
struct VisualServoingArmTransitionActionFunctor_
    : EventAgnosticActionFunctor<UAVArmSystem, LogicStateMachineT> {
  void run(UAVArmSystem &robot_system) {
    VLOG(1) << "Setting Goal for visual servoing arm connector!";
    robot_system.setGoal<VisualServoingControllerArmConnector, tf::Transform>(
        robot_system.armGoalTransform(TransformIndex));
    // Also ensure the gripper is open before going to pick objects
    robot_system.grip(false);
  }
};

template <class LogicStateMachineT>
using PickState_ = BaseState<UAVArmSystem, LogicStateMachineT,
                             PickInternalActionFunctor_<LogicStateMachineT>>;

template <class LogicStateMachineT>
class PrePickState_ : public PickState_<LogicStateMachineT> {};