/*
 * node.cpp
 *
 *  Created on: Aug 1, 2012
 *      Author: Berk Calli
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *		Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 * Node for perturbation based extremum seeking control
 */


#include <esc_perturb/perturb_esc_nd.h>
#include "esc_ros/esc_ros.h"

int main(int argc, char **argv) {

	ros::init(argc, argv, "perturb_esc_nd");
	ros::NodeHandle n("~");

	double sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_pole,comp_zero,period;

	if (!n.getParam("sin_amp", sin_amp)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_amp from the parameter server. Using the default value.");
		sin_amp = 0;
	}
	if (!n.getParam("sin_freq", sin_freq)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_freq from the parameter server. Using the default value.");
		sin_freq = 0;
	}
	if (!n.getParam("corr_gain", corr_gain)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter corr_gain from the parameter server. Using the default value.");
		corr_gain = 0;
	}
	if (!n.getParam("high_pass_pole", high_pass_pole)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter high_pass_pole from the parameter server. Using the default value.");
		high_pass_pole = 0;
	}
	if (!n.getParam("low_pass_pole", low_pass_pole)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter low_pass_pole from the parameter server. Using the default value.");
		low_pass_pole = 0;
	}
	if (!n.getParam("comp_pole", comp_pole)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_pole from the parameter server. Using the default value.");
		comp_pole = 0;
	}
	if (!n.getParam("comp_zero", comp_zero)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_zero from the parameter server. Using the default value.");
		comp_zero = 0;
	}
	if (!n.getParam("period", period)){
		ROS_WARN("[perturb_esc_nd]: Failed to get the parameter period from the parameter server. Using the default value.");
		period = 0;
	}

	ESCROS esc_ros(&n);
	PerturbESCND* perturb_esc_nd = new PerturbESCND(sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_zero,comp_pole,period);
	esc_ros.init(perturb_esc_nd);
	esc_ros.spin();

	return 0;
}