Index: /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h
===================================================================
--- /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h	(revision 10)
+++ /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h	(revision 11)
@@ -18,6 +18,7 @@
 #include <esc_common/esc.h>
 #include <string>
-#include "esc_ros/monitor.h"
-#include "esc_ros/esc_w_state.h"
+#include <esc_ros/Monitors.h>
+#include <esc_ros/StateValue.h>
+
 class ESCROS{
 protected:
@@ -39,5 +40,5 @@
 protected:
 	virtual void objValCallback(std_msgs::Float32 msg);
-	virtual void objValWithStateCallback(esc_ros::esc_w_state msg);
+	virtual void objValWithStateCallback(esc_ros::StateValue msg);
 
 };
Index: /trunk/extremum_seeking/esc_ros/msg/Monitors.msg
===================================================================
--- /trunk/extremum_seeking/esc_ros/msg/Monitors.msg	(revision 11)
+++ /trunk/extremum_seeking/esc_ros/msg/Monitors.msg	(revision 11)
@@ -0,0 +1,2 @@
+string[]  names
+float32[] values
Index: /trunk/extremum_seeking/esc_ros/msg/StateValue.msg
===================================================================
--- /trunk/extremum_seeking/esc_ros/msg/StateValue.msg	(revision 11)
+++ /trunk/extremum_seeking/esc_ros/msg/StateValue.msg	(revision 11)
@@ -0,0 +1,2 @@
+float32[] state
+float32   value
Index: unk/extremum_seeking/esc_ros/msg/esc_w_state.msg
===================================================================
--- /trunk/extremum_seeking/esc_ros/msg/esc_w_state.msg	(revision 10)
+++ 	(revision )
@@ -1,2 +1,0 @@
-float64[] states
-float32 obj_val
Index: unk/extremum_seeking/esc_ros/msg/monitor.msg
===================================================================
--- /trunk/extremum_seeking/esc_ros/msg/monitor.msg	(revision 10)
+++ 	(revision )
@@ -1,2 +1,0 @@
-float64[] monitor_values
-string[] monitor_names
Index: /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp
===================================================================
--- /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp	(revision 10)
+++ /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp	(revision 11)
@@ -49,5 +49,5 @@
 
 	if(monitor_){
-		pub_monitor_ = n_->advertise<esc_ros::monitor>("monitor_out",1);
+		pub_monitor_ = n_->advertise<esc_ros::Monitors>("monitor_out",1);
 	}
 
@@ -57,8 +57,10 @@
 }
 
-void ESCROS::objValWithStateCallback(esc_ros::esc_w_state msg)
+void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
 {
-	obj_val_ = msg.obj_val;
-	state_vec_ = msg.states;
+	obj_val_ = msg.value;
+	state_vec_.resize(msg.state.size());
+	for (size_t ii=0; ii < state_vec_.size(); ++ii)
+  	  state_vec_[ii] = msg.state[ii];
 	if(!first_obj_val_received_)
 		first_obj_val_received_ = true;
@@ -97,7 +99,11 @@
 
 			if(monitor_){
-				esc_ros::monitor msg_monitor;
-				msg_monitor.monitor_values = esc_->monitor();
-				msg_monitor.monitor_names = esc_->monitorNames();
+				esc_ros::Monitors msg_monitor;
+				std::vector<double> values = esc_->monitor();
+				
+				msg_monitor.values.resize(values.size());
+				for (size_t ii=0; ii < values.size(); ++ii)
+				  msg_monitor.values[ii] = values[ii];
+				msg_monitor.names = esc_->monitorNames();
 				pub_monitor_.publish(msg_monitor);
 			}
Index: /trunk/extremum_seeking/esc_test/CMakeLists.txt
===================================================================
--- /trunk/extremum_seeking/esc_test/CMakeLists.txt	(revision 11)
+++ /trunk/extremum_seeking/esc_test/CMakeLists.txt	(revision 11)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+rosbuild_add_executable(esc_test src/esc_test.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Index: /trunk/extremum_seeking/esc_test/Makefile
===================================================================
--- /trunk/extremum_seeking/esc_test/Makefile	(revision 11)
+++ /trunk/extremum_seeking/esc_test/Makefile	(revision 11)
@@ -0,0 +1,1 @@
+include $(shell rospack find mk)/cmake.mk
Index: /trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h
===================================================================
--- /trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h	(revision 11)
+++ /trunk/extremum_seeking/esc_test/include/esc_test/esc_test.h	(revision 11)
@@ -0,0 +1,26 @@
+#include <ros/ros.h>
+
+#include "function.h"
+
+class ESCTest
+{
+  protected:
+    ros::NodeHandle nh_;
+    ros::Publisher val_pub_, state_val_pub_;
+    ros::Subscriber vel_sub_, pos_sub_;
+    
+    ESCSystem *system_;
+    
+  protected:
+    void velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+    void positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+    void publish();
+    
+  public:
+    ESCTest() : nh_("~")
+    {
+    }
+    
+    void init(ESCSystem *system);
+    void spin();
+};
Index: /trunk/extremum_seeking/esc_test/include/esc_test/function.h
===================================================================
--- /trunk/extremum_seeking/esc_test/include/esc_test/function.h	(revision 11)
+++ /trunk/extremum_seeking/esc_test/include/esc_test/function.h	(revision 11)
@@ -0,0 +1,84 @@
+#include <limits>
+#include <exception>
+
+class ESCFunction
+{
+  public:
+    virtual std::vector<float> init() = 0;
+    virtual float value(const std::vector<float> &state) const = 0;
+};
+
+class Gauss : public ESCFunction
+{
+  private:
+    float a_, b_, c_;
+
+  public:
+    Gauss(float a=1, float b=0, float c=1) : a_(a), b_(b), c_(c) { }
+    
+    std::vector<float> init()
+    {
+      std::vector<float> state;
+      state.push_back(0);
+      return state;
+    }
+    
+    float value(const std::vector<float> &state) const
+    {
+      if (state.size() != 1)
+        throw std::runtime_error("invalid state size");
+        
+      return a_*std::exp(-(state[0]-b_)*(state[0]-b_)/(2*c_*c_));
+    }
+};
+
+class ESCSystem
+{
+  protected:
+    ESCFunction *function_;
+    std::vector<float> state_;
+        
+  public:
+    ESCSystem(ESCFunction *function) : function_(function)
+    {
+      if (!function)
+        throw std::runtime_error("no function specified");
+        
+      reset();
+    }
+    
+    void reset()
+    {
+      state_ = function_->init();
+    }
+  
+    float step(const std::vector<float> &vel)
+    {
+      if (state_.size() != vel.size())
+        throw std::runtime_error("invalid state size");
+    
+      for (size_t ii=0; ii < state_.size() && ii < vel.size(); ++ii)
+        state_[ii] += vel[ii];
+        
+      return function_->value(state_);
+    }
+    
+    float set(const std::vector<float> &pos)
+    {
+      if (state_.size() != pos.size())
+        throw std::runtime_error("invalid state size");
+    
+      state_ = pos;
+      return function_->value(state_);
+    }
+    
+    float value() const
+    {
+      return function_->value(state_);
+    }
+    
+    const std::vector<float> &state()
+    {
+      return state_;
+    }
+};
Index: /trunk/extremum_seeking/esc_test/mainpage.dox
===================================================================
--- /trunk/extremum_seeking/esc_test/mainpage.dox	(revision 11)
+++ /trunk/extremum_seeking/esc_test/mainpage.dox	(revision 11)
@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b esc_test 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
Index: /trunk/extremum_seeking/esc_test/manifest.xml
===================================================================
--- /trunk/extremum_seeking/esc_test/manifest.xml	(revision 11)
+++ /trunk/extremum_seeking/esc_test/manifest.xml	(revision 11)
@@ -0,0 +1,15 @@
+<package>
+  <description brief="esc_test">
+
+     Testing node for extremum seeking control
+
+  </description>
+  <author>Wouter Caarls</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/esc_test</url>
+  <depend package="roscpp"/>
+  <depend package="std_msgs"/>
+  <depend package="esc_ros"/>
+
+</package>
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 11)
@@ -0,0 +1,62 @@
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <esc_ros/StateValue.h>
+#include <esc_test/esc_test.h>
+
+void ESCTest::velocityCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
+{
+  system_->step(msg->data);
+  publish();
+}
+
+void ESCTest::positionCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
+{
+  system_->set(msg->data);
+  publish();
+}
+
+void ESCTest::publish()
+{
+  std_msgs::Float32 val;
+  val.data = system_->value();
+  val_pub_.publish(val);
+
+  esc_ros::StateValue state_val;
+  state_val.state = system_->state();
+  state_val.value = system_->value();
+  state_val_pub_.publish(state_val);
+}
+
+void ESCTest::init(ESCSystem *system)
+{
+  ROS_INFO("Initializing ESC test node");
+
+  system_ = system;
+  
+  val_pub_ = nh_.advertise<std_msgs::Float32>("obj_val", 1, true);
+  state_val_pub_ = nh_.advertise<esc_ros::StateValue>("esc_in", 1, true);
+  vel_sub_ = nh_.subscribe("vel_ref", 1, &ESCTest::velocityCallback, this);
+  pos_sub_ = nh_.subscribe("pos_ref", 1, &ESCTest::positionCallback, this);
+
+  publish();  
+}
+
+void ESCTest::spin()
+{
+  ROS_INFO("Spinning");
+
+  ros::spin();
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "esc_test");
+  
+  ESCTest test;
+  
+  test.init(new ESCSystem(new Gauss(-1, 2, 1)));
+  test.spin();
+  
+  return 0;
+}