Changeset 19 for trunk/extremum_seeking/esc_ros/src

Timestamp:

01/03/13 15:32:03 (12 years ago)

Author:

wcaarls

Message:

Updated extremum_seeking to revision 1177

File:

• trunk/extremum_seeking/esc_ros/src/esc_ros.cpp (modified) (6 diffs)

trunk/extremum_seeking/esc_ros/src/esc_ros.cpp

@@ -23 +23 @@
                 n_ = new ros::NodeHandle();
 
+        reset();
+        enabled_ = true;
         esc_ = esc;
+
 
         if (!n_->getParam("period", period_)){
@@ -52 +55 @@
         }
 
-        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 +67 @@
         if(!first_obj_val_received_)
                 first_obj_val_received_ = true;
-
 }
 
@@ -76 +77 @@
 }
 
+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 +102 @@
         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 +163 @@
 
         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();
         }