rfid_node.cpp
Go to the documentation of this file.
00001 
00026 #include "rfid_node.h"
00027 
00029 
00030 RfidNode::RfidNode(RfidDriverInterface *rfid_driver) :
00031     _nh_private("~"), _publish_rate(5), _rfid_driver(rfid_driver)
00032 {
00033     // get ros params
00034     std::string searched_param;
00035 
00036     // find the dev port name
00037     _nh_private.searchParam("num_device", searched_param);
00038     _nh_private.param(searched_param, _num_device, HEAD_READER_ID);
00039 
00040     // connect with the driver
00041     if (_rfid_driver->open_device(_num_device) != 0) {
00042         ROS_ERROR("[RFID_NODE] Error connecting to driver. Device: %d", _num_device);
00043         exit(-1);
00044     }
00045 
00046     // set the maximum number of labels to detect in each inventory, default 1
00047     _rfid_driver->set_num_labels(1);
00048 }
00049 
00051 
00052 RfidNode::~RfidNode()
00053 {
00054     // disconnect the driver
00055     if (_rfid_driver->close_device() != 0) {
00056         ROS_ERROR("[RFID_NODE] Error disconnecting to the driver");
00057     }
00058 }
00059 
00061 
00062 void RfidNode::init()
00063 {
00064     _write_pub = _nh_private.advertise<rfid_msgs::RfidTag>("rfid_write", 1);
00065     _rfid_write_srv = _nh_private.advertiseService("rfid_write", &RfidNode::write_card, this);
00066 }
00067 
00069 
00070 void RfidNode::spin()
00071 {
00072     while(_nh.ok()) {
00073         ros::spinOnce();
00074         _publish_rate.sleep();
00075 
00076         // detect and read rfid cards
00077         read_card();
00078     }
00079 }
00080 
00082 
00083 bool RfidNode::write_card(rfid_msgs::WriteCard::Request & req, rfid_msgs::WriteCard::Response & resp)
00084 {
00085     // copy data from message
00086     card_data card;
00087 
00088     for (int i = 0; i < req.card.id.size(); ++i) {
00089         card.UID[i] = req.card.id[i];
00090     }
00091 
00092     card.data.length = req.card.data.size();
00093 
00094     for (int i = 0; i < req.card.data.size(); ++i) {
00095         card.data.value[i] = req.card.data[i];
00096     }
00097 
00098     // check type of reader
00099     if (strcmp(_rfid_driver->get_type_reader().c_str(), HF_NAME) == 0) {
00100         // write data
00101         write_tag_data(card);
00102 
00103         // wait for writing
00104         sleep(3);
00105     }
00106     else {
00107         ROS_ERROR("[RFID_NODE] Error: cannot write rfid card");
00108 
00109         return false;
00110     }
00111 
00112     return true;
00113 }
00114 
00116 
00117 int RfidNode::read_card()
00118 {
00119     int error = 0;
00120     int num_cards = _rfid_driver->inventory();
00121 
00122     rfid_msgs::RfidTag msg;
00123 
00124     // if there are cards near, read them
00125     if (num_cards > 0) {
00126         // check type of reader
00127         if (strcmp(_rfid_driver->get_type_reader().c_str(), HF_NAME) == 0) {
00128             // get the uid label
00129             string id = _rfid_driver->get_uid_label();
00130 
00131             if (id != "0") {
00132                 set_data(read_tag_data(get_num_blocks()));
00133 
00134                 // publish data
00135                 ROS_INFO("[RFID_NODE] RFID id: %s", _rfid_card.UID);
00136                 ROS_INFO("[RFID_NODE] RFID data (size: %d): %s", _rfid_card.data.length, _rfid_card.data.value);
00137 
00138                 // fill the message
00139                 msg.header.stamp = ros::Time::now();
00140                 msg.header.frame_id = "/rfid";
00141                 for (int i = 0; i < strlen(_rfid_card.UID); ++i) {
00142                     msg.id.push_back(_rfid_card.UID[i]);
00143                 }
00144                 for (int i = 0; i < strlen((char *) _rfid_card.data.value); ++i) {
00145                     msg.data.push_back(_rfid_card.data.value[i]);
00146                 }
00147 
00148                 _write_pub.publish(msg);
00149             }
00150         }
00151         else {
00152             ROS_ERROR("[RFID_NODE] Error: Unknown RFID reader");
00153         }
00154     }
00155     else {
00156         ROS_DEBUG("[RFID_NODE] No cards detected");
00157 
00158         error = -1;
00159     }
00160 
00161     return error;
00162 }
00163 
00165 
00166 void RfidNode::set_data(card_data card)
00167 {
00168     bcopy((const void*) &card, (void*) &_rfid_card, sizeof(card_data));
00169 }
00170 
00172 
00173 card_data RfidNode::read_tag_data(int num_blocks)
00174 {
00175     unsigned char data_aux[MAX_LENGTH] = "";
00176     unsigned char data_read[BLOCK_SIZE_HF];
00177 
00178     int iterator = 0;
00179     for (int i = 0; i <= num_blocks; i++) {
00180         _rfid_driver->read_hf(data_read, (unsigned char) i);
00181 
00182         for (int j = 0; j < BLOCK_SIZE_HF; j++) {
00183             data_aux[iterator + j] = data_read[j];
00184         }
00185 
00186         iterator += BLOCK_SIZE_HF;
00187     }
00188 
00189     _rfid_card.data.length = strlen((char*) data_aux);
00190 
00191     for (int i = 0; i < MAX_LENGTH; i++) {
00192         _rfid_card.data.value[i] = data_aux[i];
00193     }
00194     ROS_DEBUG("[RFID_NODE] HF data read: %s", _rfid_card.data.value);
00195 
00196     // get the uid label
00197     string ID_aux = _rfid_driver->get_uid_label();
00198     int len = ID_aux.length();
00199 
00200     char* ID_tag = new char[len + 1];
00201     ID_aux.copy(ID_tag, len, 0);
00202     ID_tag[len] = '\0';
00203 
00204     strcpy(_rfid_card.UID, ID_tag);
00205 
00206     return _rfid_card;
00207 }
00208 
00210 
00211 void RfidNode::write_tag_data(card_data card)
00212 {
00213     int dir = 0;
00214     unsigned char datos_aux[BLOCK_SIZE_HF];
00215     string uid = "";
00216 
00217     // wait until read at least a card
00218     while(_rfid_driver->inventory() == 0) {
00219         ROS_INFO("[RFID_NODE] Waiting card to write");
00220     }
00221 
00222     // get the uid label
00223     uid = _rfid_driver->get_uid_label();
00224     int len = strlen((char*) card.data.value);
00225 
00226     // write data
00227     int i = 0;
00228     while(i < len) {
00229         for (int j = 0; j < BLOCK_SIZE_HF; j++) {
00230             if (i < len) {
00231                 datos_aux[j] = card.data.value[i];
00232                 i++;
00233             }
00234         }
00235         _rfid_driver->write_hf(datos_aux, (unsigned char) dir, uid);
00236         dir++;
00237     }
00238 }
00239 
00241 
00242 int RfidNode::get_num_blocks()
00243 {
00244     int num_blocks = 0;
00245     unsigned char data[BLOCK_SIZE_HF] = "";
00246 
00247     _rfid_driver->read_hf(data, (unsigned char) 0);
00248 
00249     num_blocks = atoi((char*) data);
00250 
00251     return num_blocks;
00252 }
00253 


rfid
Author(s): Raul Perula-Martinez
autogenerated on Wed Apr 1 2015 10:17:22