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
{
    idle,

    moving_to_ready,

    calculating_path,

    giving_cup,

    cancelling,

    stopped
};

enum class ArmError
{
    safety_concern,

    no_path_found,

    driver_error
};

std::string robot_controller_state_to_string(RobotControllerState state);

std::string arm_error_to_string(ArmError error);

class RobotController
{
public:
    enum class Event
    {
        move_to_ready,

        pick_up_cup,

        stop,

        resume,

        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);

    void set_state_change_callback(
        const std::function<void(RobotControllerState)>& cb);

private:
    void set_state(RobotControllerState state);

    void position_reached();

    void event_handler();

    void move_to_ready_handler();

    void pick_up_cup_handler();

    void stop_handler();

    void resume_handler();

    void cancel_handler();

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

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

    const double speed = 100;

    const double distance_from_cup = 0.06;

    const double lift_distance = 0.10;

    const double angle_step_size = 5;

    const double euclidean_step_size = 0.05;

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

    std::function<void(RobotControllerState)> state_callback;

    RobotControllerState prev_state = RobotControllerState::moving_to_ready;

    RobotControllerState state = RobotControllerState::moving_to_ready;

    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