Changeset 19


Ignore:
Timestamp:
01/03/13 15:32:03 (11 years ago)
Author:
wcaarls
Message:

Updated extremum_seeking to revision 1177

Location:
trunk/extremum_seeking/esc_ros
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/extremum_seeking/esc_ros/include/esc_ros/esc_ros.h

    r11 r19  
    1616#include <std_msgs/Float32.h>
    1717#include <std_msgs/Float32MultiArray.h>
     18#include <std_msgs/Bool.h>
     19#include <std_msgs/Empty.h>
    1820#include <esc_common/esc.h>
    1921#include <string>
     
    2426protected:
    2527        ESC* esc_;
    26         ros::Publisher pub_ref_, pub_monitor_;
    27         ros::Subscriber sub_obj_val_;
     28        ros::Publisher pub_ref_, pub_monitor_, pub_stopped_;
     29        ros::Subscriber sub_obj_val_, sub_enable_;
    2830        ros::NodeHandle* n_;
    2931        double obj_val_;
    3032        std::vector<double> state_vec_;
    31         bool initialized_, monitor_, first_obj_val_received_;
     33        bool initialized_, monitor_, first_obj_val_received_,enabled_, reference_zeroed_;
    3234        double period_;
     35        unsigned int opt_dim_;
    3336
    3437public:
     
    3740        virtual void step();
    3841        virtual void spin();
     42        virtual void reset();
     43        virtual void enableCallback(std_msgs::Bool msg);
    3944        virtual ~ESCROS(){};
    4045protected:
  • trunk/extremum_seeking/esc_ros/src/esc_ros.cpp

    r11 r19  
    2323                n_ = new ros::NodeHandle();
    2424
     25        reset();
     26        enabled_ = true;
    2527        esc_ = esc;
     28
    2629
    2730        if (!n_->getParam("period", period_)){
     
    5255        }
    5356
    54         initialized_ = true;
    55         first_obj_val_received_ = false;
    56 
     57        sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
     58        pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
    5759}
    5860
     
    6567        if(!first_obj_val_received_)
    6668                first_obj_val_received_ = true;
    67 
    6869}
    6970
     
    7677}
    7778
     79void ESCROS::reset(){
     80
     81        initialized_ = true;
     82        first_obj_val_received_ = false;
     83        reference_zeroed_ = false;
     84        opt_dim_ = 0;
     85}
     86
     87void ESCROS::enableCallback(std_msgs::Bool msg)
     88{
     89        if (msg.data){
     90                reset();
     91                esc_->reset();
     92                enabled_ = true;
     93                ROS_INFO("[esc_ros]: ESC enabled.");
     94        }
     95        else
     96                enabled_ = false;
     97}
     98
    7899void ESCROS::step()
    79100{
     
    81102        if(initialized_){
    82103                if(first_obj_val_received_){
    83                         std::vector<double> out;
    84                         if(esc_->getInputType() == esc_->inputValue)
    85                                 out = esc_->step(obj_val_);
    86                         else if (esc_->getInputType() == esc_->inputStateValue)
    87                                 out = esc_->step(state_vec_,obj_val_);
    88                         else
    89                                 ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
     104                        if(enabled_){
     105                                std::vector<double> out;
     106                                if(esc_->getInputType() == esc_->inputValue)
     107                                        out = esc_->step(obj_val_);
     108                                else if (esc_->getInputType() == esc_->inputStateValue)
     109                                        out = esc_->step(state_vec_,obj_val_);
     110                                else
     111                                        ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
    90112
    91                         if (out.empty()){
    92                                 return;
     113                                if (out.empty()){
     114                                        return;
     115                                }
     116                                if(opt_dim_ == 0){
     117                                        opt_dim_ = out.size();
     118                                }
     119
     120                                std_msgs::Float32MultiArray msg;
     121                                for(size_t i=0;i<out.size();i++)
     122                                        msg.data.push_back(out[i]);
     123                                pub_ref_.publish(msg);
     124
     125                                if(monitor_){
     126                                        esc_ros::Monitors msg_monitor;
     127                                        std::vector<double> values = esc_->monitor();
     128
     129                                        msg_monitor.values.resize(values.size());
     130                                        for (size_t ii=0; ii < values.size(); ++ii)
     131                                          msg_monitor.values[ii] = values[ii];
     132                                        msg_monitor.names = esc_->monitorNames();
     133                                        pub_monitor_.publish(msg_monitor);
     134                                }
     135                        }
     136                        else if(!reference_zeroed_){
     137
     138                                std_msgs::Float32MultiArray msg;
     139
     140                                if (opt_dim_ != 0){
     141                                        msg.data.resize(opt_dim_);
     142
     143                                        for(size_t i = 0; i<opt_dim_; i++)
     144                                                msg.data[i] = 0;
     145
     146                                        pub_ref_.publish(msg);
     147                                        opt_dim_ = 0;
     148                                }
     149                                reference_zeroed_ = true;
     150
    93151                        }
    94152
    95                         std_msgs::Float32MultiArray msg;
    96                         for(size_t i=0;i<out.size();i++)
    97                                 msg.data.push_back(out[i]);
    98                         pub_ref_.publish(msg);
     153                }
    99154
    100                         if(monitor_){
    101                                 esc_ros::Monitors msg_monitor;
    102                                 std::vector<double> values = esc_->monitor();
    103                                
    104                                 msg_monitor.values.resize(values.size());
    105                                 for (size_t ii=0; ii < values.size(); ++ii)
    106                                   msg_monitor.values[ii] = values[ii];
    107                                 msg_monitor.names = esc_->monitorNames();
    108                                 pub_monitor_.publish(msg_monitor);
    109                         }
    110                 }
    111155        }
    112156        else
     
    119163
    120164        while (n_->ok()) {
     165                if(esc_->isStoppingConditionsMet()){
     166                        std_msgs::Empty msg_stopped;
     167                        pub_stopped_.publish(msg_stopped);
     168                        enabled_ = false;
     169                }
     170                step();
    121171                ros::spinOnce();
    122                 step();
    123172                loop_rate.sleep();
    124173        }
Note: See TracChangeset for help on using the changeset viewer.