| [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 | |
|---|
| 14 | ESCROS::ESCROS(ros::NodeHandle* n){ |
|---|
| 15 | n_ = n; |
|---|
| 16 | initialized_ = false; |
|---|
| 17 | |
|---|
| 18 | } |
|---|
| 19 | |
|---|
| 20 | void 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] | 59 | void 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 | |
|---|
| 70 | void 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 | |
|---|
| 78 | void 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 | |
|---|
| 116 | void 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 | } |
|---|
