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

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

Imported extremum_seeking at revision 987

File size: 2.9 KB
Line 
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_){
51                pub_monitor_ = n_->advertise<esc_ros::monitor>("monitor_out",1);
52        }
53
54        initialized_ = true;
55        first_obj_val_received_ = false;
56
57}
58
59void ESCROS::objValWithStateCallback(esc_ros::esc_w_state msg)
60{
61        obj_val_ = msg.obj_val;
62        state_vec_ = msg.states;
63        if(!first_obj_val_received_)
64                first_obj_val_received_ = true;
65
66}
67
68void ESCROS::objValCallback(std_msgs::Float32 msg)
69{
70        obj_val_ = msg.data;
71        if(!first_obj_val_received_)
72                first_obj_val_received_ = true;
73
74}
75
76void ESCROS::step()
77{
78
79        if(initialized_){
80                if(first_obj_val_received_){
81                        std::vector<double> out;
82                        if(esc_->getInputType() == esc_->inputValue)
83                                out = esc_->step(obj_val_);
84                        else if (esc_->getInputType() == esc_->inputStateValue)
85                                out = esc_->step(state_vec_,obj_val_);
86                        else
87                                ROS_WARN("Invalid input type for the extremum seeking controller. The step function will not be executed.");
88
89                        if (out.empty()){
90                                return;
91                        }
92
93                        std_msgs::Float32MultiArray msg;
94                        for(size_t i=0;i<out.size();i++)
95                                msg.data.push_back(out[i]);
96                        pub_ref_.publish(msg);
97
98                        if(monitor_){
99                                esc_ros::monitor msg_monitor;
100                                msg_monitor.monitor_values = esc_->monitor();
101                                msg_monitor.monitor_names = esc_->monitorNames();
102                                pub_monitor_.publish(msg_monitor);
103                        }
104                }
105        }
106        else
107                ROS_WARN("ESC class is not initialized. Its functions will not be executed.");
108}
109
110void ESCROS::spin()
111{
112        ros::Rate loop_rate(1.0/period_);
113
114        while (n_->ok()) {
115                ros::spinOnce();
116                step();
117                loop_rate.sleep();
118        }
119}
Note: See TracBrowser for help on using the repository browser.