AbstractController.hpp

#ifndef ABSTRACTCONTROLLER_HPP
#define ABSTRACTCONTROLLER_HPP

#include <vector>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "urdf/model.h"

#include "VirtualServo.hpp"
#include <Simulation/VirtualSerialInterface.hpp>

namespace HLR
{
namespace Simulation
{
class AbstractController : public VirtualSerialInterface
{
protected:
    AbstractController(const std::string& request_topic,
                       const std::string& respond_topic,
                       const std::string& robot_description);

    virtual void emergency_stop();

    virtual void parse_message(const std::string& message) = 0;

    virtual bool arm_is_moving();

private:
    void write(const std::string& message) override;

    std::string read() override;

    void handle_message_cb(const std_msgs::String::ConstPtr& message);

    void start_update_cycles();

protected:
    bool response_available;

    std::vector<VirtualServo> servos;

    urdf::Model model;

    std::string response;

private:
    std_msgs::String feedback;

    ros::NodeHandle ros_node;

    ros::Subscriber message_subscriber;

    ros::Publisher feedback_publisher;
};
}
}

#endif /* ABSTRACTCONTROLLER_HPP */