#include <ros/ros.h>

#include "function.h"

class ESCTest
{
  protected:
    ros::NodeHandle nh_;
    ros::Publisher val_pub_, state_val_pub_;
    ros::Subscriber vel_sub_, pos_sub_;
    
    ESCSystem *system_;
    
  protected:
    void velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
    void positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
    void publish();
    
  public:
    ESCTest() : nh_("~")
    {
    }
    
    void init(ESCSystem *system);
    void spin();
};