1 | /* |
---|
2 | * node.cpp |
---|
3 | * |
---|
4 | * Created on: Aug 1, 2012 |
---|
5 | * Author: Berk Calli |
---|
6 | * Organization: Delft Biorobotics Lab., Delft University of Technology |
---|
7 | * Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl |
---|
8 | * |
---|
9 | * Node for perturbation based extremum seeking control |
---|
10 | */ |
---|
11 | |
---|
12 | |
---|
13 | #include <esc_perturb/perturb_esc_nd.h> |
---|
14 | #include "esc_ros/esc_ros.h" |
---|
15 | |
---|
16 | int main(int argc, char **argv) { |
---|
17 | |
---|
18 | ros::init(argc, argv, "perturb_esc_nd"); |
---|
19 | ros::NodeHandle n("~"); |
---|
20 | |
---|
21 | double sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_pole,comp_zero,period; |
---|
22 | |
---|
23 | if (!n.getParam("sin_amp", sin_amp)){ |
---|
24 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_amp from the parameter server. Using the default value."); |
---|
25 | sin_amp = 0; |
---|
26 | } |
---|
27 | if (!n.getParam("sin_freq", sin_freq)){ |
---|
28 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter sin_freq from the parameter server. Using the default value."); |
---|
29 | sin_freq = 0; |
---|
30 | } |
---|
31 | if (!n.getParam("corr_gain", corr_gain)){ |
---|
32 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter corr_gain from the parameter server. Using the default value."); |
---|
33 | corr_gain = 0; |
---|
34 | } |
---|
35 | if (!n.getParam("high_pass_pole", high_pass_pole)){ |
---|
36 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter high_pass_pole from the parameter server. Using the default value."); |
---|
37 | high_pass_pole = 0; |
---|
38 | } |
---|
39 | if (!n.getParam("low_pass_pole", low_pass_pole)){ |
---|
40 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter low_pass_pole from the parameter server. Using the default value."); |
---|
41 | low_pass_pole = 0; |
---|
42 | } |
---|
43 | if (!n.getParam("comp_pole", comp_pole)){ |
---|
44 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_pole from the parameter server. Using the default value."); |
---|
45 | comp_pole = 0; |
---|
46 | } |
---|
47 | if (!n.getParam("comp_zero", comp_zero)){ |
---|
48 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter comp_zero from the parameter server. Using the default value."); |
---|
49 | comp_zero = 0; |
---|
50 | } |
---|
51 | if (!n.getParam("period", period)){ |
---|
52 | ROS_WARN("[perturb_esc_nd]: Failed to get the parameter period from the parameter server. Using the default value."); |
---|
53 | period = 0; |
---|
54 | } |
---|
55 | |
---|
56 | ESCROS esc_ros(&n); |
---|
57 | PerturbESCND* perturb_esc_nd = new PerturbESCND(sin_amp,sin_freq,corr_gain,high_pass_pole,low_pass_pole,comp_zero,comp_pole,period); |
---|
58 | esc_ros.init(perturb_esc_nd); |
---|
59 | esc_ros.spin(); |
---|
60 | |
---|
61 | return 0; |
---|
62 | } |
---|