Index: trunk/extremum_seeking/esc_test/src/esc_test.cpp
===================================================================
--- trunk/extremum_seeking/esc_test/src/esc_test.cpp	(revision 11)
+++ trunk/extremum_seeking/esc_test/src/esc_test.cpp	(revision 13)
@@ -7,12 +7,12 @@
 void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->step(msg->data);
-  publish();
+  vel_ = msg->data;
+  pos_.clear();
 }
 
 void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
 {
-  system_->set(msg->data);
-  publish();
+  pos_ = msg->data;
+  vel_.clear();
 }
 
@@ -39,6 +39,4 @@
   vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
   pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
-
-  publish();  
 }
 
@@ -47,5 +45,16 @@
   ROS_INFO("Spinning");
 
-  ros::spin();
+  ros::Rate loop_rate(100);
+  while (nh_.ok())
+  {
+    if (!vel_.empty())
+      system_->step(vel_);
+    else if (!pos_.empty())
+      system_->set(pos_);
+    publish();
+
+    loop_rate.sleep();
+    ros::spinOnce();
+  }  
 }
 
@@ -54,7 +63,21 @@
   ros::init(argc, argv, "esc_test");
   
+  ESCFunction *function;
+  
+  if (argc > 1 && strcmp(argv[1], "-2") == 0)
+  {
+    std::vector<float> mean, sigma;
+    mean.push_back(0.5);
+    mean.push_back(0.2);
+    sigma.push_back(0.15);
+    sigma.push_back(0.15);
+    function = new Gauss2D(2, -1, mean, sigma);
+  }
+  else
+    function = new Gauss1D(2.5, -1.5, 0.4, 0.27/sqrt(2.0));
+  
   ESCTest test;
   
-  test.init(new ESCSystem(new Gauss(-1, 2, 1)));
+  test.init(new ESCSystem(function));
   test.spin();