source: trunk/extremum_seeking/esc_ros/src/esc_ros.cpp

/*
 * esc_ros.cpp
 *
 *  Created on: Jul 26, 2012
 *      Author: Berk Calli, Wouter Caarls
 *      Organization: Delft Biorobotics Lab., Delft University of Technology
 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
 *
 *      ROS wrapper for extremum seeking control nodes.
 */
#include <esc_ros/esc_ros.h>


ESCROS::ESCROS(ros::NodeHandle* n){
        n_ = n;
        initialized_ = false;

}

void ESCROS::init(ESC* esc)
{
        if(!n_)
                n_ = new ros::NodeHandle();

        reset();
        enabled_ = true;
        esc_ = esc;


        if (!n_->getParam("period", period_)){
                ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
                period_ = 0;
        }

        if (!n_->getParam("monitor", monitor_)){
                ROS_WARN("[nn_esc_1D]: Failed to get the parameter monitor from the parameter server. Using the default value.");
                monitor_ = false;
        }
        if(esc_->getInputType() == esc_->inputValue)
                sub_obj_val_ = n_->subscribe("obj_val", 1, &ESCROS::objValCallback, this);
        else if (esc_->getInputType() == esc_->inputStateValue)
                sub_obj_val_ = n_->subscribe("esc_in", 1, &ESCROS::objValWithStateCallback, this);
        else
                ROS_WARN("The input value is not specified. The subscriber will not be created.");

        if(esc_->getOutputType() == esc_->outputVelocity)
                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("vel_ref",1);
        else if(esc_->getOutputType() == esc_->outputPosition)
                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("pos_ref",1);
        else
                ROS_WARN("The output value is not specified. The output of the ESC can not be published.");

        if(monitor_){
                pub_monitor_ = n_->advertise<esc_ros::Monitors>("monitor_out",1);
        }

        sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
        pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
}

void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
{
        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;
}

void ESCROS::objValCallback(std_msgs::Float32 msg)
{
        obj_val_ = msg.data;
        if(!first_obj_val_received_)
                first_obj_val_received_ = true;

}

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()
{

        if(initialized_){
                if(first_obj_val_received_){
                        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(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;

                        }

                }

        }
        else
                ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
}

void ESCROS::spin()
{
        ros::Rate loop_rate(1.0/period_);

        while (n_->ok()) {
                if(esc_->isStoppingConditionsMet()){
                        std_msgs::Empty msg_stopped;
                        pub_stopped_.publish(msg_stopped);
                        enabled_ = false;
                }
                step();
                ros::spinOnce();
                loop_rate.sleep();
        }
}