RobotDriverHandler.hpp

#ifndef KINEMATICS_ROSROBOTDRIVERHANDLER_HPP
#define KINEMATICS_ROSROBOTDRIVERHANDLER_HPP

#include <atomic>
#include <unordered_map>

#include <ros/ros.h>
#include "std_msgs/UInt64.h"
#include "RobotDriverHandler.hpp"
#include <Kinematics/Matrix.hpp>

namespace HLR
{
namespace Kinematics
{
class RobotDriverHandler
{
public:
    RobotDriverHandler();

    virtual ~RobotDriverHandler() = default;

    RobotDriverHandler(const RobotDriverHandler& rdh) = delete;

    RobotDriverHandler(RobotDriverHandler&& rdh) = delete;

    RobotDriverHandler& operator=(const RobotDriverHandler& rdh) = delete;

    RobotDriverHandler& operator=(RobotDriverHandler&& rdh) = delete;

    Matrix<double, 5, 1> get_arm_state() const noexcept;

    void move(const std::vector<Matrix<double, 5, 1>>& path, double speed);

    void stop();

    void set_move_complete_handler(const std::function<void()>& handler);

private:
    void sub_move_complete(const std_msgs::UInt64::ConstPtr& id);

    std::function<void()> move_complete_handler;

    ros::NodeHandle nh;

    ros::Subscriber id_subscriber;

    ros::ServiceClient driver_stop;

    ros::ServiceClient driver_add;

    std::atomic<Matrix<double, 5, 1>> current_arm_state;

    std::uint64_t id_to_wait = std::numeric_limits<std::uint64_t>::max();

    std::unordered_map<std::uint64_t, Matrix<double, 5, 1>> moves_being_handled;
};

} // namespace Kinematics
} // namespace HLR

#endif