/*
 * esc_ros.h
 *
 *  Created on: Jul 26, 2012
 *      Author: Berk Calli, Wouter Caarls
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *	Header of the ROS wrapper for extremum seeking control nodes.
 */

#ifndef ESC_ROS_H_
#define ESC_ROS_H_

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <esc_common/esc.h>
#include <string>
#include "esc_ros/monitor.h"
#include "esc_ros/esc_w_state.h"
class ESCROS{
protected:
	ESC* esc_;
	ros::Publisher pub_ref_, pub_monitor_;
	ros::Subscriber sub_obj_val_;
	ros::NodeHandle* n_;
	double obj_val_;
	std::vector<double> state_vec_;
	bool initialized_, monitor_, first_obj_val_received_;
	double period_;

public:
	ESCROS(ros::NodeHandle* n=NULL);
	virtual void init(ESC* esc);
	virtual void step();
	virtual void spin();
	virtual ~ESCROS(){};
protected:
	virtual void objValCallback(std_msgs::Float32 msg);
	virtual void objValWithStateCallback(esc_ros::esc_w_state msg);

};




#endif /* ESC_ROS_H_ */