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

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

Updated extremum_seeking to revision 1001

File size: 3.2 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
25        esc_ = esc;
26
27        if (!n_->getParam("period", period_)){
28                ROS_WARN("[nn_esc_1D]: Failed to get the parameter period from the parameter server. Using the default value.");
29                period_ = 0;
30        }
31
32        if (!n_->getParam("monitor", monitor_)){
33                ROS_WARN("[nn_esc_1D]: Failed to get the parameter monitor from the parameter server. Using the default value.");
34                monitor_ = false;
35        }
36        if(esc_->getInputType() == esc_->inputValue)
37                sub_obj_val_ = n_->subscribe("obj_val", 1, &ESCROS::objValCallback, this);
38        else if (esc_->getInputType() == esc_->inputStateValue)
39                sub_obj_val_ = n_->subscribe("esc_in", 1, &ESCROS::objValWithStateCallback, this);
40        else
41                ROS_WARN("The input value is not specified. The subscriber will not be created.");
42
43        if(esc_->getOutputType() == esc_->outputVelocity)
44                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("vel_ref",1);
45        else if(esc_->getOutputType() == esc_->outputPosition)
46                pub_ref_ = n_->advertise<std_msgs::Float32MultiArray>("pos_ref",1);
47        else
48                ROS_WARN("The output value is not specified. The output of the ESC can not be published.");
49
50        if(monitor_){
[11]51                pub_monitor_ = n_->advertise<esc_ros::Monitors>("monitor_out",1);
[5]52        }
53
54        initialized_ = true;
55        first_obj_val_received_ = false;
56
57}
58
[11]59void ESCROS::objValWithStateCallback(esc_ros::StateValue msg)
[5]60{
[11]61        obj_val_ = msg.value;
62        state_vec_.resize(msg.state.size());
63        for (size_t ii=0; ii < state_vec_.size(); ++ii)
64          state_vec_[ii] = msg.state[ii];
[5]65        if(!first_obj_val_received_)
66                first_obj_val_received_ = true;
67
68}
69
70void ESCROS::objValCallback(std_msgs::Float32 msg)
71{
72        obj_val_ = msg.data;
73        if(!first_obj_val_received_)
74                first_obj_val_received_ = true;
75
76}
77
78void ESCROS::step()
79{
80
81        if(initialized_){
82                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.");
90
91                        if (out.empty()){
92                                return;
93                        }
94
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);
99
100                        if(monitor_){
[11]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();
[5]108                                pub_monitor_.publish(msg_monitor);
109                        }
110                }
111        }
112        else
113                ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
114}
115
116void ESCROS::spin()
117{
118        ros::Rate loop_rate(1.0/period_);
119
120        while (n_->ok()) {
121                ros::spinOnce();
122                step();
123                loop_rate.sleep();
124        }
125}
Note: See TracBrowser for help on using the repository browser.