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

Last change on this file since 19 was 19, checked in by wcaarls, 12 years ago

Updated extremum_seeking to revision 1177
File size: 4.1 KB
RevLine 
[5]1/*
2 * esc_ros.cpp
3 *
4 *  Created on: Jul 26, 2012
5 *      Author: Berk Calli, Wouter Caarls
6 *      Organization: Delft Biorobotics Lab., Delft University of Technology
7 *              Contact info: b.calli@tudelft.nl, web: www.dbl.tudelft.nl
8 *
9 *      ROS wrapper for extremum seeking control nodes.
10 */
11#include <esc_ros/esc_ros.h>
12
13
14ESCROS::ESCROS(ros::NodeHandle* n){
15        n_ = n;
16        initialized_ = false;
17
18}
19
20void ESCROS::init(ESC* esc)
21{
22        if(!n_)
23                n_ = new ros::NodeHandle();
24
[19]25        reset();
26        enabled_ = true;
[5]27        esc_ = esc;
28
[19]29
[5]30        if (!n_->getParam("period", period_)){
31                ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
32                period_ = 0;
33        }
34
35        if (!n_->getParam("monitor", monitor_)){
36                ROS_WARN("[nn_esc_1D]: Failed to get the parameter monitor from the parameter server. Using the default value.");
37                monitor_ = false;
38        }
39        if(esc_->getInputType() == esc_->inputValue)
40                sub_obj_val_ = n_->subscribe("obj_val", 1, &ESCROS::objValCallback, this);
41        else if (esc_->getInputType() == esc_->inputStateValue)
42                sub_obj_val_ = n_->subscribe("esc_in", 1, &ESCROS::objValWithStateCallback, this);
43        else
44                ROS_WARN("The input value is not specified. The subscriber will not be created.");
45
46        if(esc_->getOutputType() == esc_->outputVelocity)
47                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("vel_ref",1);
48        else if(esc_->getOutputType() == esc_->outputPosition)
49                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("pos_ref",1);
50        else
51                ROS_WARN("The output value is not specified. The output of the ESC can not be published.");
52
53        if(monitor_){
[11]54                pub_monitor_ = n_->advertise<esc_ros::Monitors>("monitor_out",1);
[5]55        }
56
[19]57        sub_enable_ = n_->subscribe("enable", 1, &ESCROS::enableCallback, this);
58        pub_stopped_ = n_->advertise<std_msgs::Empty>("stopping_conditions_met",1);
[5]59}
60
[11]61void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
[5]62{
[11]63        obj_val_ = msg.value;
64        state_vec_.resize(msg.state.size());
65        for (size_t ii=0; ii < state_vec_.size(); ++ii)
66          state_vec_[ii] = msg.state[ii];
[5]67        if(!first_obj_val_received_)
68                first_obj_val_received_ = true;
69}
70
71void ESCROS::objValCallback(std_msgs::Float32 msg)
72{
73        obj_val_ = msg.data;
74        if(!first_obj_val_received_)
75                first_obj_val_received_ = true;
76
77}
78
[19]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
[5]99void ESCROS::step()
100{
101
102        if(initialized_){
103                if(first_obj_val_received_){
[19]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.");
[5]112
[19]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                                }
[5]135                        }
[19]136                        else if(!reference_zeroed_){
[5]137
[19]138                                std_msgs::Float32MultiArray msg;
[5]139
[19]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
[5]151                        }
[19]152
[5]153                }
[19]154
[5]155        }
156        else
157                ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
158}
159
160void ESCROS::spin()
161{
162        ros::Rate loop_rate(1.0/period_);
163
164        while (n_->ok()) {
[19]165                if(esc_->isStoppingConditionsMet()){
166                        std_msgs::Empty msg_stopped;
167                        pub_stopped_.publish(msg_stopped);
168                        enabled_ = false;
169                }
170                step();
[5]171                ros::spinOnce();
172                loop_rate.sleep();
173        }
174}
Note: See TracBrowser for help on using the repository browser.