Driver.hpp

#ifndef KINEMATICS_RV2AJ_DRIVER_HPP
#define KINEMATICS_RV2AJ_DRIVER_HPP

#include <ros/ros.h>
#include <std_msgs/String.h>
#include "HLR/RV2AJDriverClear.h"
#include "HLR/RV2AJDriverContinue.h"
#include "HLR/RV2AJDriverMB4.h"
#include "HLR/RV2AJDriverPause.h"
#include "HLR/RV2AJDriverQueueAdd.h"
#include "HLR/RV2AJDriverQueueCount.h"

#include <condition_variable>
#include <mutex>
#include <queue>
#include <thread>

namespace HLR
{
namespace Kinematics
{
namespace RV2AJ
{
class Driver
{
public:
    Driver();

    virtual ~Driver();

    Driver(const Driver& d) = delete;

    Driver(Driver&& d) = delete;

    Driver& operator=(const Driver& d) = delete;

    Driver& operator=(Driver&& d) = delete;

    bool cont(HLR::RV2AJDriverContinue::Request& req,
              HLR::RV2AJDriverContinue::Response& res);

    bool pause(HLR::RV2AJDriverPause::Request& req,
               HLR::RV2AJDriverPause::Response& res);

    bool clear(HLR::RV2AJDriverClear::Request& req,
               HLR::RV2AJDriverClear::Response& res);

    bool queue_count(HLR::RV2AJDriverQueueCount::Request& req,
                     HLR::RV2AJDriverQueueCount::Response& res);

    bool queue_add(HLR::RV2AJDriverQueueAdd::Request& req,
                   HLR::RV2AJDriverQueueAdd::Response& res);

private:
    struct Command
    {
        std::string to_mb4() const;

        double base;
        double shoulder;
        double elbow;
        double j4;
        double wrist;
        double wrist_rotate;
        double j7;
        double speed;
    };

    enum class State
    {
        cont = 0,
        pause = 1,
        exit = 2
    };

    void work();

    ros::NodeHandle nh;

    ros::Publisher pub_mb4;

    ros::ServiceServer srv_continue;

    ros::ServiceServer srv_pause;

    ros::ServiceServer srv_clear;

    ros::ServiceServer srv_queue_count;

    ros::ServiceServer srv_queue_add;

    std::thread worker;

    State worker_state = State::cont;

    std::mutex worker_state_m;

    std::queue<Command> commands;

    std::mutex commands_m;

    std::condition_variable commands_cv;
};
} // namespace RV2AJ
} // namespace Kinematics
} // namespace HLR

#endif /* KINEMATICS_RV2AJ_DRIVER_HPP */