Changeset 13 for trunk/extremum_seeking/esc_test/src/esc_test.cpp

Timestamp:

08/29/12 16:52:52 (12 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1017

File:

• trunk/extremum_seeking/esc_test/src/esc_test.cpp (modified) (4 diffs)

trunk/extremum_seeking/esc_test/src/esc_test.cpp

@@ -7 +7 @@
 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 +39 @@
   vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
   pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
-
-  publish();  
 }
 
@@ -47 +45 @@
   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 +63 @@
   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();