RobotController.hpp

#ifndef KINEMATICS_ROBOTCONTROLLER_HPP
#define KINEMATICS_ROBOTCONTROLLER_HPP

#include <ros/ros.h>
#include "Planners/Planner.hpp"
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <Kinematics/ConcurrentQueue.hpp>
#include <Kinematics/Matrix.hpp>

#include <mutex>
#include <stdexcept>
#include <thread>
#include <unordered_set>

#include "RobotDriverHandler.hpp"

namespace HLR
{
namespace Kinematics
{
struct EnvironmentObject
{
    std::uint32_t id;

    Matrix<double, 3, 1> position;

    Matrix<double, 3, 1> velocity;

    bool is_cup;

    bool has_speed;

    CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3> polyhedron;
};

enum class RobotControllerState
{
    init,

    idle,

    moving_to_ready,

    moving_to_cup,

    picking_up_cup_stage1,

    picking_up_cup_stage2,

    moving_cup,

    waiting_for_confirmation,

    stopped
};

enum class ArmError
{
    safety_concern,

    no_path_found,

    driver_error
};

class RobotController
{
public:
    enum class Event
    {
        move_to_ready,

        move_to_cup,

        pick_up_cup_stage1,

        pick_up_cup_stage2,

        move_cup,

        move_stop,

        move_continue,

        move_cancel,

        terminate
    };

    RobotController(RobotDriverHandler& driver);

    virtual ~RobotController();

    RobotController(const RobotController& rc) = delete;

    RobotController(RobotController&& rc) = delete;

    RobotController& operator=(const RobotController& rc) = delete;

    RobotController& operator=(RobotController&& rc) = delete;

    RobotControllerState get_state() const;

    void pick_up_cup(std::uint32_t id);

    void move_stop();

    void move_continue();

    void move_cancel();

    void remove_object(const EnvironmentObject& obj);

    void add_object(EnvironmentObject&& obj);

    void set_error_callback(const std::function<void(ArmError)>& cb);

private:
    void set_state(RobotControllerState state);

    void position_reached();

    void move_to_position(const Matrix<double, 3, 1>& pos,
                          const Planners::Planner& planner,
                          double speed);

    void event_handler();

    void move_to_ready_handler();

    void pick_up_cup_handler();

    void pick_up_cup_stage1_handler();

    void pick_up_cup_stage2_handler();

    void move_cup_handler();

    void move_stop_handler();

    void move_continue_handler();

    void move_cancel_handler();

    const Matrix<double, 3, 1> target_position = {{0.2}, {0.0}, {0.4}};

    const Matrix<double, 3, 1> ready_pos = {{0.357}, {0.216506}, {0}};

    const double ground_level = 0;

    const double distance_from_cup = 0.12;

    const double angle_step_size = 360;

    const double euclidean_step_size = 0.05;

    std::function<void(ArmError)> error_callback;

    bool is_carrying = false;

    RobotControllerState state = RobotControllerState::init;

    mutable std::mutex state_m;

    Matrix<double, 3, 1> cup_position;

    std::unordered_map<std::uint32_t, EnvironmentObject> objects;

    mutable std::recursive_mutex objects_m;

    std::thread worker;

    ConcurrentQueue<Event> events;

    RobotDriverHandler& driver;
};

} // namespace Kinematics
} // namespace HLR

#endif