00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00029 #include "sr_ronex_drivers/cod_decod/cod_decod_std_io.hpp"
00030 #include <sstream>
00031 #include <string>
00032 #include <vector>
00033
00034 PLUGINLIB_EXPORT_CLASS(sr_cod_decod::CodDecodStdIo, sr_cod_decod::CodDecod);
00035
00036 namespace sr_cod_decod
00037 {
00038 CodDecodStdIo::CodDecodStdIo()
00039 :CodDecod(),
00040 n_digital_outputs_(0),
00041 n_analog_outputs_(0),
00042 n_digital_inputs_(0),
00043 n_analog_inputs_(0),
00044 n_PWM_outputs_(0),
00045 command_size_(0),
00046 status_size_(0),
00047 digital_input_state_publisher_(NULL),
00048 analog_input_state_publisher_(NULL)
00049 {
00050 }
00051
00052 void CodDecodStdIo::construct(hardware_interface::HardwareInterface *hw, EtherCAT_SlaveHandler *sh,
00053 int n_digital_outputs, int n_analog_outputs, int n_digital_inputs, int n_analog_inputs,
00054 int n_PWM_outputs)
00055 {
00056 CodDecod::construct(hw, sh, n_digital_outputs, n_analog_outputs, n_digital_inputs, n_analog_inputs, n_PWM_outputs);
00057 n_digital_outputs_ = n_digital_outputs;
00058 n_analog_outputs_ = n_analog_outputs;
00059 n_digital_inputs_ = n_digital_inputs;
00060 n_analog_inputs_ = n_analog_inputs;
00061 n_PWM_outputs_ = n_PWM_outputs;
00062
00063 int dig_out_size = 0;
00064 if (n_digital_outputs_ > 0 )
00065 dig_out_size = ((n_digital_outputs_/16 + 1) * 2);
00066
00067 command_size_ = dig_out_size + (n_PWM_outputs_ * 4) + (n_analog_outputs_ * 2);
00068
00069 int dig_in_size = 0;
00070 if (n_digital_inputs_ > 0 )
00071 dig_in_size = ((n_digital_inputs_/16 + 1) * 2);
00072
00073 status_size_ = dig_in_size + (n_analog_inputs_ * 2);
00074
00075 node_ = ros::NodeHandle();
00076
00077
00078 boost::shared_ptr<sr_ronex_msgs::BoolArray> d_out_ptr(new sr_ronex_msgs::BoolArray());
00079 d_out_ptr->data.clear();
00080 for (unsigned i = 0; i < n_digital_outputs_; ++i)
00081 {
00082 d_out_ptr->data.push_back(false);
00083 }
00084 digital_output_.set(d_out_ptr);
00085
00086
00087 boost::shared_ptr<std_msgs::UInt16MultiArray> a_out_ptr(new std_msgs::UInt16MultiArray());
00088 a_out_ptr->data.clear();
00089 for (unsigned i = 0; i < n_analog_outputs_; ++i)
00090 {
00091 a_out_ptr->data.push_back(0x0000);
00092 }
00093 analog_output_.set(a_out_ptr);
00094
00095
00096 boost::shared_ptr<std_msgs::UInt16MultiArray> PWM_out_ptr(new std_msgs::UInt16MultiArray());
00097 PWM_out_ptr->data.clear();
00098 for (unsigned i = 0; i < (n_PWM_outputs_ * 2); ++i)
00099 {
00100 PWM_out_ptr->data.push_back(0x0000);
00101 }
00102 PWM_output_.set(PWM_out_ptr);
00103
00104
00105 char buff[200];
00106 string topic;
00107
00108 snprintf(buff, sizeof(buff), "device_0x%08X_0x%08X_digital_outputs_command",
00109 sh_->get_product_code(),
00110 sh_->get_serial());
00111 topic = buff;
00112 sub_digital_output_command_ = node_.subscribe<sr_ronex_msgs::BoolArray>
00113 (topic, 1, &CodDecodStdIo::digitalOutputCommandCB, this);
00114
00115
00116 snprintf(buff, sizeof(buff), "device_0x%08X_0x%08X_analog_outputs_command",
00117 sh_->get_product_code(),
00118 sh_->get_serial());
00119 topic = buff;
00120 sub_analog_output_command_ = node_.subscribe<std_msgs::UInt16MultiArray>
00121 (topic, 1, &CodDecodStdIo::analogOutputCommandCB, this);
00122
00123 snprintf(buff, sizeof(buff), "device_0x%08X_0x%08X_PWM_outputs_command",
00124 sh_->get_product_code(),
00125 sh_->get_serial());
00126 topic = buff;
00127 sub_PWM_output_command_ = node_.subscribe<std_msgs::UInt16MultiArray>
00128 (topic, 1, &CodDecodStdIo::PWMOutputCommandCB, this);
00129
00130
00131 snprintf(buff, sizeof(buff), "device_0x%08X_0x%08X_digital_inputs_state",
00132 sh_->get_product_code(),
00133 sh_->get_serial());
00134 topic = buff;
00135 digital_input_state_publisher_.reset(
00136 new realtime_tools::RealtimePublisher<sr_ronex_msgs::BoolArray>(node_, topic, 1));
00137
00138
00139 snprintf(buff, sizeof(buff), "device_0x%08X_0x%08X_analog_inputs_state",
00140 sh_->get_product_code(),
00141 sh_->get_serial());
00142 topic = buff;
00143 analog_input_state_publisher_.reset(
00144 new realtime_tools::RealtimePublisher<std_msgs::UInt16MultiArray>(node_, topic, 1));
00145 }
00146
00147 void CodDecodStdIo::update(unsigned char *status_buffer)
00148 {
00149 unsigned char *buff_ptr;
00150
00151
00152 buff_ptr = status_buffer;
00153
00154 d_in_.data.clear();
00155
00156
00157 for (unsigned i = 0; i < n_digital_inputs_; i++)
00158 {
00159 if (buff_ptr[0] & (0x01 << (i % 8))) d_in_.data.push_back(true);
00160 else d_in_.data.push_back(false);
00161
00162 if ((i + 1) % 8 == 0) buff_ptr++;
00163 }
00164
00165
00166 if (digital_input_state_publisher_ && digital_input_state_publisher_->trylock())
00167 {
00168 digital_input_state_publisher_->msg_ = d_in_;
00169 digital_input_state_publisher_->unlockAndPublish();
00170 }
00171
00172
00173
00174 buff_ptr = status_buffer + ((n_digital_inputs_/16 + 1) * 2);
00175
00176 a_in_.data.clear();
00177
00178
00179 for (unsigned i = 0; i < n_analog_inputs_; i++)
00180 {
00181 a_in_.data.push_back(*(reinterpret_cast<uint16_t *>(buff_ptr)));
00182
00183 buff_ptr += 2;
00184 }
00185
00186
00187 if (analog_input_state_publisher_ && analog_input_state_publisher_->trylock())
00188 {
00189 analog_input_state_publisher_->msg_ = a_in_;
00190 analog_input_state_publisher_->unlockAndPublish();
00191 }
00192
00193
00194 char buff[300];
00195 char aux[3];
00196 snprintf(buff, sizeof(buff), "");
00197 for (unsigned int i = 0; i < status_size_; i++)
00198 {
00199 snprintf(aux, sizeof(aux), "%02x", static_cast<uint16_t>(status_buffer[i]));
00200 snprintf(buff, sizeof(buff), aux, 3);
00201 }
00202 if (status_size_ > 0)
00203 {
00204 ROS_DEBUG("Stat buffer %02d: %s", sh_->get_ring_position(), buff);
00205 }
00206 }
00207
00208 void CodDecodStdIo::build_command(unsigned char *command_buffer)
00209 {
00210 unsigned char *buff_ptr;
00211 buff_ptr = command_buffer;
00212
00213
00214 boost::shared_ptr<sr_ronex_msgs::BoolArray> d_out_ptr;
00215 digital_output_.get(d_out_ptr);
00216
00217
00218 for (size_t i = 0; i < ((n_digital_outputs_/16 + 1) * 2); i++)
00219 {
00220 buff_ptr[i] = 0;
00221 }
00222
00223
00224 for (size_t i = 0; i < d_out_ptr->data.size(); i++)
00225 {
00226 if (d_out_ptr->data.at(i))
00227 (*(reinterpret_cast<uint8_t *>(buff_ptr))) |= (0x01 << (i % 8));
00228 if ((i + 1) % 8 == 0)
00229 ++buff_ptr;
00230 }
00231
00232
00233
00234 buff_ptr = command_buffer + ((n_digital_outputs_/16 + 1) * 2);
00235
00236
00237 boost::shared_ptr<std_msgs::UInt16MultiArray> PWM_out_ptr;
00238 PWM_output_.get(PWM_out_ptr);
00239
00240
00241 for (size_t i = 0; i < PWM_out_ptr->data.size(); i++)
00242 {
00243 *(reinterpret_cast<uint16_t *>(buff_ptr)) = PWM_out_ptr->data.at(i);
00244 buff_ptr += 2;
00245 }
00246
00247
00248
00249 buff_ptr = command_buffer + ((n_digital_outputs_/16 + 1) * 2) + (n_PWM_outputs_ * 4);
00250
00251
00252 boost::shared_ptr<std_msgs::UInt16MultiArray> a_out_ptr;
00253 analog_output_.get(a_out_ptr);
00254
00255
00256 for (size_t i = 0; i < a_out_ptr->data.size(); i++)
00257 {
00258 *(reinterpret_cast<uint16_t *>(buff_ptr)) = a_out_ptr->data.at(i);
00259 buff_ptr += 2;
00260 }
00261
00262
00263 char buff[300];
00264 char aux[3];
00265 snprintf(buff, sizeof(buff), "");
00266 for (unsigned int i = 0; i < command_size_; i++)
00267 {
00268 snprintf(aux, sizeof(aux), "%02x", static_cast<uint16_t>(command_buffer[i]));
00269 snprintf(buff, sizeof(buff), aux, 3);
00270 }
00271 if (command_size_ > 0)
00272 {
00273 ROS_DEBUG("Cmd buffer %02d: %s", sh_->get_ring_position(), buff);
00274 }
00275
00276
00277 }
00278 void CodDecodStdIo::digitalOutputCommandCB(const sr_ronex_msgs::BoolArrayConstPtr& msg)
00279 {
00280 if (msg->data.size() == n_digital_outputs_)
00281 {
00282 boost::shared_ptr<sr_ronex_msgs::BoolArray> d_out_ptr(new sr_ronex_msgs::BoolArray());
00283 d_out_ptr->data.clear();
00284 for (unsigned int i = 0; i < n_digital_outputs_; ++i)
00285 {
00286 d_out_ptr->data.push_back(msg->data.at(i));
00287 }
00288 digital_output_.set(d_out_ptr);
00289 }
00290 else
00291 {
00292 ROS_ERROR("Wrong number of digital outputs. Must be: %d", n_digital_outputs_);
00293 }
00294 }
00295
00296 void CodDecodStdIo::analogOutputCommandCB(const std_msgs::UInt16MultiArrayConstPtr& msg)
00297 {
00298 if (msg->data.size() == n_analog_outputs_)
00299 {
00300 boost::shared_ptr<std_msgs::UInt16MultiArray> a_out_ptr(new std_msgs::UInt16MultiArray());
00301 a_out_ptr->data.clear();
00302 for (unsigned int i = 0; i < n_analog_outputs_; ++i)
00303 {
00304 a_out_ptr->data.push_back(msg->data.at(i));
00305 }
00306 analog_output_.set(a_out_ptr);
00307 }
00308 else
00309 {
00310 ROS_ERROR("Wrong number of analog outputs. Must be: %d", n_analog_outputs_);
00311 }
00312 }
00313
00314 void CodDecodStdIo::PWMOutputCommandCB(const std_msgs::UInt16MultiArrayConstPtr& msg)
00315 {
00316 if (msg->data.size() == n_PWM_outputs_ * 2)
00317 {
00318 boost::shared_ptr<std_msgs::UInt16MultiArray> PWM_out_ptr(new std_msgs::UInt16MultiArray());
00319 PWM_out_ptr->data.clear();
00320 for (unsigned int i = 0; i < (n_PWM_outputs_ * 2); ++i)
00321 {
00322
00323
00324 if (!(i & 0x0001) )
00325 {
00326 if (msg->data.at(i) == 0xFFFF)
00327 {
00328 PWM_out_ptr->data.push_back(0xFFFE);
00329 }
00330 else
00331 {
00332 PWM_out_ptr->data.push_back(msg->data.at(i));
00333 }
00334 }
00335
00336
00337 else
00338 {
00339 if (msg->data.at(i) > (PWM_out_ptr->data.at(i - 1) + 1))
00340 {
00341 PWM_out_ptr->data.push_back(PWM_out_ptr->data.at(i - 1) + 1);
00342 }
00343 else
00344 {
00345 PWM_out_ptr->data.push_back(msg->data.at(i));
00346 }
00347 }
00348 }
00349 PWM_output_.set(PWM_out_ptr);
00350 }
00351 else
00352 {
00353 ROS_ERROR("Wrong number of PWM outputs. Must be: %d. Remember that you need 2 UINT values for every output "
00354 "(Period, ON-time)", n_PWM_outputs_);
00355 }
00356 }
00357 void CodDecodStdIo::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00358 diagnostic_updater::DiagnosticStatusWrapper &d)
00359 {
00360 }
00361 void CodDecodStdIo::setPinAsDigitalInput(sr_cod_decod_std_io::DigitalIo pin)
00362 {
00363
00364
00365 boost::shared_ptr<sr_ronex_msgs::BoolArray> d_out_ptr(new sr_ronex_msgs::BoolArray());
00366 boost::shared_ptr<sr_ronex_msgs::BoolArray> current_d_out_ptr;
00367 digital_output_.get(current_d_out_ptr);
00368 d_out_ptr->data = current_d_out_ptr->data;
00369
00370 d_out_ptr->data.at(pin * 2) = true;
00371 digital_output_.set(d_out_ptr);
00372 }
00373
00374 bool CodDecodStdIo::digitalInputToBool(sr_cod_decod_std_io::DigitalIo input_pin)
00375 {
00376
00377 return static_cast<bool>(d_in_.data.at(input_pin));
00378 }
00379
00380 void CodDecodStdIo::digitalInputToBool(sr_cod_decod_std_io::DigitalIo input_pin, bool *where_to_store_it)
00381 {
00382
00383
00384 *where_to_store_it = static_cast<bool>(d_in_.data.at(input_pin));
00385 }
00386
00387 double CodDecodStdIo::analogInputToDouble(sr_cod_decod_std_io::AnalogInput input_pin)
00388 {
00389
00390 return static_cast<double>(a_in_.data.at(input_pin));
00391 }
00392
00393 void CodDecodStdIo::analogInputToDouble(sr_cod_decod_std_io::AnalogInput input_pin, double *where_to_store_it)
00394 {
00395
00396
00397 *where_to_store_it = static_cast<double>(a_in_.data.at(input_pin));
00398 }
00399
00400 uint16_t CodDecodStdIo::analogInputToUint16(sr_cod_decod_std_io::AnalogInput input_pin)
00401 {
00402
00403 return a_in_.data.at(input_pin);
00404 }
00405
00406 void CodDecodStdIo::analogInputToUint16(sr_cod_decod_std_io::AnalogInput input_pin, uint16_t *where_to_store_it)
00407 {
00408
00409
00410 *where_to_store_it = a_in_.data.at(input_pin);
00411 }
00412
00413 int CodDecodStdIo::analogInputToInt(sr_cod_decod_std_io::AnalogInput input_pin)
00414 {
00415
00416 return static_cast<int>(a_in_.data.at(input_pin));
00417 }
00418
00419 void CodDecodStdIo::analogInputToInt(sr_cod_decod_std_io::AnalogInput input_pin, int *where_to_store_it)
00420 {
00421
00422
00423 *where_to_store_it = static_cast<int>(a_in_.data.at(input_pin));
00424 }
00425
00426 void CodDecodStdIo::signToDigitalOutput(sr_cod_decod_std_io::DigitalIo output_pin, double value)
00427 {
00428 boolToDigitalOutput(output_pin, (value < 0.0));
00429 }
00430
00431 void CodDecodStdIo::boolToDigitalOutput(sr_cod_decod_std_io::DigitalIo output_pin, bool value)
00432 {
00433
00434
00435 boost::shared_ptr<sr_ronex_msgs::BoolArray> d_out_ptr(new sr_ronex_msgs::BoolArray());
00436 boost::shared_ptr<sr_ronex_msgs::BoolArray> current_d_out_ptr;
00437 digital_output_.get(current_d_out_ptr);
00438 d_out_ptr->data = current_d_out_ptr->data;
00439
00440 d_out_ptr->data.at(output_pin * 2) = false;
00441
00442 d_out_ptr->data.at(output_pin * 2 + 1) = value;
00443 digital_output_.set(d_out_ptr);
00444 }
00445
00446 void CodDecodStdIo::doubleToAnalogOutput(sr_cod_decod_std_io::AnalogOutput output_pin, double value)
00447 {
00448 uint16ToAnalogOutput(output_pin, static_cast<uint16_t>(abs(value)));
00449 }
00450
00451 void CodDecodStdIo::uint16ToAnalogOutput(sr_cod_decod_std_io::AnalogOutput output_pin, uint16_t value)
00452 {
00453
00454
00455 boost::shared_ptr<std_msgs::UInt16MultiArray> analog_out_ptr(new std_msgs::UInt16MultiArray());
00456 boost::shared_ptr<std_msgs::UInt16MultiArray> current_analog_out_ptr;
00457 analog_output_.get(current_analog_out_ptr);
00458 analog_out_ptr->data = current_analog_out_ptr->data;
00459
00460 analog_out_ptr->data.at(output_pin) = value;
00461 analog_output_.set(analog_out_ptr);
00462 }
00463
00464 void CodDecodStdIo::doubleToPWMOutput(sr_cod_decod_std_io::DigitalIo output_pin, uint16_t PWM_period,
00465 double PWM_ON_time)
00466 {
00467 uint16ToPWMOutput(output_pin, PWM_period, static_cast<uint16_t>
00468 (abs(PWM_ON_time) > static_cast<double>(PWM_period + 1) ? static_cast<double>(PWM_period + 1) : abs(PWM_ON_time) +
00469 0.5));
00470 }
00471
00472 void CodDecodStdIo::uint16ToPWMOutput(
00473 sr_cod_decod_std_io::DigitalIo output_pin, uint16_t PWM_period, uint16_t PWM_ON_time)
00474 {
00475
00476
00477 boost::shared_ptr<std_msgs::UInt16MultiArray> PWM_out_ptr(new std_msgs::UInt16MultiArray());
00478 boost::shared_ptr<std_msgs::UInt16MultiArray> current_PWM_out_ptr;
00479 PWM_output_.get(current_PWM_out_ptr);
00480 PWM_out_ptr->data = current_PWM_out_ptr->data;
00481
00482 PWM_out_ptr->data.at(output_pin * 2) = PWM_period;
00483
00484 PWM_out_ptr->data.at(output_pin * 2 + 1) = static_cast<uint16_t>(PWM_ON_time > (PWM_period + 1) ? (PWM_period + 1) :
00485 PWM_ON_time);
00486 PWM_output_.set(PWM_out_ptr);
00487 }
00488
00489 }