Index: /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h
===================================================================
--- /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h	(revision 18)
+++ /trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h	(revision 19)
@@ -16,4 +16,6 @@
 #include <std_msgs/Float32.h>
 #include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
 #include <esc_common/esc.h>
 #include <string>
@@ -24,11 +26,12 @@
 protected:
 	ESC* esc_;
-	ros::Publisher pub_ref_, pub_monitor_;
-	ros::Subscriber sub_obj_val_;
+	ros::Publisher pub_ref_, pub_monitor_, pub_stopped_;
+	ros::Subscriber sub_obj_val_, sub_enable_;
 	ros::NodeHandle* n_;
 	double obj_val_;
 	std::vector<double> state_vec_;
-	bool initialized_, monitor_, first_obj_val_received_;
+	bool initialized_, monitor_, first_obj_val_received_,enabled_, reference_zeroed_;
 	double period_;
+	unsigned int opt_dim_;
 
 public:
@@ -37,4 +40,6 @@
 	virtual void step();
 	virtual void spin();
+	virtual void reset();
+	virtual void enableCallback(std_msgs::Bool msg);
 	virtual ~ESCROS(){};
 protected:
Index: /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp
===================================================================
--- /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp	(revision 18)
+++ /trunk/extremum_seeking/esc_ros/src/esc_ros.cpp	(revision 19)
@@ -23,5 +23,8 @@
 		n_ = new ros::NodeHandle();
 
+	reset();
+	enabled_ = true;
 	esc_ = esc;
+
 
 	if (!n_->getParam("period", period_)){
@@ -52,7 +55,6 @@
 	}
 
-	initialized_ = true;
-	first_obj_val_received_ = false;
-
+	sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
+	pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
 }
 
@@ -65,5 +67,4 @@
 	if(!first_obj_val_received_)
 		first_obj_val_received_ = true;
-
 }
 
@@ -76,4 +77,24 @@
 }
 
+void ESCROS::reset(){
+
+	initialized_ = true;
+	first_obj_val_received_ = false;
+	reference_zeroed_ = false;
+	opt_dim_ = 0;
+}
+
+void ESCROS::enableCallback(std_msgs::Bool msg)
+{
+	if (msg.data){
+		reset();
+		esc_->reset();
+		enabled_ = true;
+		ROS_INFO("[esc_ros]: ESC enabled.");
+	}
+	else
+		enabled_ = false;
+}
+
 void ESCROS::step()
 {
@@ -81,32 +102,55 @@
 	if(initialized_){
 		if(first_obj_val_received_){
-			std::vector<double> out;
-			if(esc_->getInputType() == esc_->inputValue)
-				out = esc_->step(obj_val_);
-			else if (esc_->getInputType() == esc_->inputStateValue)
-				out = esc_->step(state_vec_,obj_val_);
-			else
-				ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
+			if(enabled_){
+				std::vector<double> out;
+				if(esc_->getInputType() == esc_->inputValue)
+					out = esc_->step(obj_val_);
+				else if (esc_->getInputType() == esc_->inputStateValue)
+					out = esc_->step(state_vec_,obj_val_);
+				else
+					ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
 
-			if (out.empty()){
-				return;
+				if (out.empty()){
+					return;
+				}
+				if(opt_dim_ == 0){
+					opt_dim_ = out.size();
+				}
+
+				std_msgs::Float32MultiArray msg;
+				for(size_t i=0;i<out.size();i++)
+					msg.data.push_back(out[i]);
+				pub_ref_.publish(msg);
+
+				if(monitor_){
+					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);
+				}
+			}
+			else if(!reference_zeroed_){
+
+				std_msgs::Float32MultiArray msg;
+
+				if (opt_dim_ != 0){
+					msg.data.resize(opt_dim_);
+
+					for(size_t i = 0; i<opt_dim_; i++)
+						msg.data[i] = 0;
+
+					pub_ref_.publish(msg);
+					opt_dim_ = 0;
+				}
+				reference_zeroed_ = true;
+
 			}
 
-			std_msgs::Float32MultiArray msg;
-			for(size_t i=0;i<out.size();i++)
-				msg.data.push_back(out[i]);
-			pub_ref_.publish(msg);
+		}
 
-			if(monitor_){
-				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);
-			}
-		}
 	}
 	else
@@ -119,6 +163,11 @@
 
 	while (n_->ok()) {
+		if(esc_->isStoppingConditionsMet()){
+			std_msgs::Empty msg_stopped;
+			pub_stopped_.publish(msg_stopped);
+			enabled_ = false;
+		}
+		step();
 		ros::spinOnce();
-		step();
 		loop_rate.sleep();
 	}