RosRobotDriverHandler.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 RosRobotDriverHandler : public RobotDriverHandler
{
public:
    RosRobotDriverHandler();

    virtual ~RosRobotDriverHandler() = default;

    RosRobotDriverHandler(const RosRobotDriverHandler& rdh) = delete;

    RosRobotDriverHandler(RosRobotDriverHandler&& rdh) = delete;

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

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

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

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

    void stop() override;

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 = 0;

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

} // namespace Kinematics
} // namespace HLR

#endif