00001 #include "evarobot_infrared/evarobot_infrared.h"
00002
00003 int i_error_code = 0;
00004
00005
00006
00007
00008
00009
00010
00011
00012 IMInfrared::IMInfrared(int id,
00013 boost::shared_ptr<IMADC> _adc,
00014 boost::shared_ptr<IMDynamicReconfig> _dynamic_params):i_id(id), adc(_adc),
00015 b_is_alive(false), dynamic_params(_dynamic_params)
00016 {
00017 stringstream ss;
00018
00019
00020 ss << "ir" << this->i_id;
00021 this->pub_inf = this->n.advertise<sensor_msgs::Range>(ss.str().c_str(), 10);
00022
00023
00024 this->updater.setHardwareID("SHARP");
00025 this->updater.add(ss.str().c_str(), this, &IMInfrared::ProduceDiagnostics);
00026
00027 this->min_freq = this->dynamic_params->d_min_freq;
00028 this->max_freq = this->dynamic_params->d_max_freq;
00029 this->pub_inf_freq = new diagnostic_updater::HeaderlessTopicDiagnostic(ss.str().c_str(), this->updater,
00030 diagnostic_updater::FrequencyStatusParam(&this->min_freq, &this->max_freq, 0.1, 10));
00031
00032
00033
00034 ss.str("");
00035 ss << this->n.resolveName(n.getNamespace(), true) << "/ir" << this->i_id << "_link";
00036
00037 this->inf_msg.header.frame_id = ss.str();
00038 this->inf_msg.radiation_type = 1;
00039
00040 }
00041
00042 IMInfrared::~IMInfrared()
00043 {
00044 delete pub_inf_freq;
00045 }
00046
00047
00048 bool IMInfrared::ReadRange()
00049 {
00050 int i_raw_data = 0;
00051 bool b_ret = false;
00052 float f_range = 0.0;
00053
00054 try{
00055 i_raw_data = this->adc->ReadChannel(this->i_id);
00056 }catch(int e){
00057 ROS_INFO(GetErrorDescription(e).c_str());
00058 i_error_code = e;
00059 }
00060 f_range = 0.01 * ((1 / (this->dynamic_params->d_par_a * (double)i_raw_data + this->dynamic_params->d_par_b))
00061 - this->dynamic_params->d_par_k);
00062
00063 if(f_range > this->dynamic_params->d_max_range || f_range < 0)
00064 f_range = this->dynamic_params->d_max_range;
00065
00066
00067 if(i_raw_data < 10)
00068 {
00069 this->b_is_alive = false;
00070 b_ret = false;
00071 }
00072 else
00073 {
00074 this->b_is_alive = true;
00075 b_ret = true;
00076 }
00077
00078
00079 this->inf_msg.range = f_range;
00080 this->inf_msg.header.stamp = ros::Time::now();
00081 this->inf_msg.field_of_view = this->dynamic_params->d_fov;
00082 this->inf_msg.min_range = this->dynamic_params->d_min_range;
00083 this->inf_msg.max_range = this->dynamic_params->d_max_range;
00084
00085 return b_ret;
00086 }
00087
00088 void IMInfrared::Publish()
00089 {
00090 if(this->pub_inf.getNumSubscribers() > 0 || this->dynamic_params->b_always_on)
00091 {
00092 this->pub_inf.publish(this->inf_msg);
00093 this->pub_inf_freq->tick();
00094 }
00095 }
00096
00097 void IMInfrared::ProduceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
00098 {
00099 if(i_error_code<0)
00100 {
00101 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "%s", GetErrorDescription(i_error_code).c_str());
00102 i_error_code = 0;
00103 }
00104 else if(this->b_is_alive)
00105 {
00106 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "INFRARED%d is alive!", this->i_id);
00107 }
00108 else
00109 {
00110 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "INFRARED%d is NOT alive!", this->i_id);
00111 }
00112 }
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122 void IMDynamicReconfig::CallbackReconfigure(evarobot_infrared::ParamsConfig &config, uint32_t level)
00123 {
00124 ROS_DEBUG("EvarobotInfrared: Updating Sonar Params...");
00125
00126 this->d_fov = config.field_of_view;
00127 this->d_min_range = config.minRange;
00128 this->d_max_range = config.maxRange;
00129 this->b_always_on = config.alwaysOn;
00130 this->d_par_k = config.parK;
00131 this->d_par_a = config.parA;
00132 this->d_par_b = config.parB;
00133 }
00134
00135 IMDynamicReconfig::IMDynamicReconfig():n_private("~")
00136 {
00137
00138 n_private.param("alwaysOn", this->b_always_on, false);
00139 n_private.param("field_of_view", this->d_fov, 0.7);
00140 n_private.param("minRange", this->d_min_range, 0.0);
00141 n_private.param("maxRange", this->d_max_range, 5.0);
00142 n_private.param("maxFreq", this->d_max_freq, 10.0);
00143 n_private.param("minFreq", this->d_min_freq, 0.2);
00144 n_private.param("parK", this->d_par_k, 0.42);
00145 n_private.param("parA", this->d_par_a, 0.0000473);
00146 n_private.param("parB", this->d_par_b, -0.0013461);
00147
00148 ROS_DEBUG("EvarobotInfrared: b_always_on: %d", this->b_always_on);
00149 ROS_DEBUG("EvarobotInfrared: d_fov: %f", this->d_fov);
00150 ROS_DEBUG("EvarobotInfrared: d_min_range: %f", this->d_min_range);
00151 ROS_DEBUG("EvarobotInfrared: d_max_range: %f", this->d_max_range);
00152
00153
00154
00155 this->f = boost::bind(&IMDynamicReconfig::CallbackReconfigure, this, _1, _2);
00156 this->srv.setCallback(this->f);
00157 }
00158
00159
00160 int main(int argc, char **argv)
00161 {
00162 unsigned char u_c_spi_mode;
00163 vector<IMInfrared *> infrared;
00164
00165
00166 vector<int> T_i_channels;
00167 double d_frequency;
00168
00169 string str_driver_path;
00170
00171 int i_spi_mode;
00172 int i_spi_speed;
00173 int i_spi_bits;
00174 int i_adc_bits;
00175
00176
00177
00178 ros::init(argc, argv, "/evarobot_infrared");
00179 ros::NodeHandle n;
00180
00181 n.param<string>("evarobot_infrared/driverPath", str_driver_path, "/dev/spidev0.0");
00182 n.param("evarobot_infrared/spiMode", i_spi_mode, 0);
00183 n.param("evarobot_infrared/spiSpeed", i_spi_speed, 1000000);
00184 n.param("evarobot_infrared/spiBits", i_spi_bits, 8);
00185 n.param("evarobot_infrared/adcBits", i_adc_bits, 12);
00186
00187 if(!n.getParam("evarobot_infrared/frequency", d_frequency))
00188 {
00189 ROS_INFO(GetErrorDescription(-100).c_str());
00190 i_error_code = -100;
00191 }
00192
00193 if(i_adc_bits > 16)
00194 {
00195 ROS_INFO(GetErrorDescription(-101).c_str());
00196 i_error_code = -101;
00197 }
00198
00199 n.getParam("evarobot_infrared/channelNo", T_i_channels);
00200
00201 if(T_i_channels.size() > MAX_IR)
00202 {
00203 ROS_INFO(GetErrorDescription(-102).c_str());
00204 i_error_code = -102;
00205 }
00206
00207 ROS_DEBUG("EvarobotInfrared: Number of ir sensors: %d", T_i_channels.size());
00208
00209
00210
00211 ros::Rate loop_rate(d_frequency);
00212
00213
00214 switch(i_spi_mode)
00215 {
00216 case 0:
00217 {
00218 u_c_spi_mode = SPI_MODE_0;
00219 break;
00220 }
00221
00222 case 1:
00223 {
00224 u_c_spi_mode = SPI_MODE_1;
00225 break;
00226 }
00227
00228 case 2:
00229 {
00230 u_c_spi_mode = SPI_MODE_2;
00231 break;
00232 }
00233
00234 case 3:
00235 {
00236 u_c_spi_mode = SPI_MODE_3;
00237 break;
00238 }
00239
00240 default:
00241 {
00242 ROS_INFO(GetErrorDescription(-103).c_str());
00243 i_error_code = -103;
00244 }
00245 }
00246
00247 IMSPI * p_im_spi;
00248 boost::shared_ptr<IMADC> p_im_adc;
00249 boost::shared_ptr<IMDynamicReconfig> dyn_params(new IMDynamicReconfig);
00250 try{
00251 p_im_spi = new IMSPI(str_driver_path, u_c_spi_mode, i_spi_speed, i_spi_bits);
00252 p_im_adc = boost::shared_ptr<IMADC>(new IMADC(p_im_spi, i_adc_bits));
00253 }catch(int e){
00254 ROS_INFO(GetErrorDescription(e).c_str());
00255 i_error_code = e;
00256 return 0;
00257 }
00258
00259 for(uint i = 0; i < T_i_channels.size(); i++)
00260 {
00261 if(T_i_channels[i] < MAX_IR)
00262 {
00263 IMInfrared * dummy_inf = new IMInfrared(T_i_channels[i], p_im_adc, dyn_params);
00264 infrared.push_back(dummy_inf);
00265 }
00266 else
00267 {
00268 ROS_INFO(GetErrorDescription(-104).c_str());
00269 i_error_code = -104;
00270 }
00271 }
00272
00273 while(ros::ok())
00274 {
00275 for(uint i = 0; i < T_i_channels.size(); i++)
00276 {
00277 if(infrared[i]->ReadRange())
00278 {
00279 infrared[i]->Publish();
00280 }
00281
00282 infrared[i]->updater.update();
00283
00284 }
00285
00286 ros::spinOnce();
00287 loop_rate.sleep();
00288 }
00289
00290 return 0;
00291 }
00292