Driver.hpp

#ifndef KINEMATICS_RV2AJ_DRIVER_HPP
#define KINEMATICS_RV2AJ_DRIVER_HPP

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

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

#include "Serial.hpp"

namespace HLR
{
namespace Kinematics
{
namespace RV2AJ
{
class Driver
{
public:
    Driver(bool simulation,
           const std::optional<std::string>& serial_path = std::nullopt,
           bool debug = false);

    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::array<std::string, 3> to_mb4() const;

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

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

    void tx(const Command& cmd);

    void tx(const std::string& mb4);

    void rx();

    void rx_sub_callback(const std_msgs::String::ConstPtr& msg);

    void work();

    void work_reply();

    bool debug;

    std::size_t device_count = 0;

    uint64_t cmd_id = 0;

    ros::NodeHandle nh;

    std::mutex tx_m;

    ros::Publisher tx_pub;

    ros::Publisher tx_id_pub;

    bool tx_abort = false;

    std::mutex tx_abort_m;

    std::mutex rx_m;

    bool rx_block = true;

    std::mutex rx_block_m;

    std::condition_variable rx_block_cv;

    ros::Subscriber rx_sub;

    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;

    std::thread worker_reply;

    State worker_reply_state = State::cont;

    std::mutex worker_reply_state_m;

    std::queue<std::string> replies;

    std::mutex replies_m;

    std::condition_variable replies_cv;

    Serial serial;
};
} // namespace RV2AJ
} // namespace Kinematics
} // namespace HLR

#endif /* KINEMATICS_RV2AJ_DRIVER_HPP */