test_sdo.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK Canopen Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00022 //----------------------------------------------------------------------
00023 
00024 #include <icl_hardware_can/tCanDeviceDummy.h>
00025 
00026 #include <icl_hardware_canopen/CanOpenController.h>
00027 #include <icl_hardware_canopen/DS301Group.h>
00028 #include <icl_hardware_canopen/SDO.h>
00029 
00030 #include <icl_hardware_canopen/Logging.h>
00031 #include <icl_hardware_canopen/exceptions.h>
00032 
00033 #include <boost/test/unit_test.hpp>
00034 #include <boost/thread/thread.hpp>
00035 using namespace icl_hardware::canopen_schunk;
00036 
00037 
00038 BOOST_AUTO_TEST_SUITE(ts_sdo)
00039 
00040 BOOST_AUTO_TEST_CASE (sdo_download)
00041 {
00042   // Initializing
00043   icl_core::logging::initialize();
00044   icl_core::logging::setLogLevel(icl_core::logging::eLL_TRACE);
00045   LOGGING_INFO(CanOpen, "-----------------------------------" << endl <<
00046                         "-----Running SDO download test-----" << endl <<
00047                         "-----------------------------------" << endl);
00048 
00049   CanOpenController my_controller("Dummy");
00050   boost::shared_ptr<icl_hardware::can::tCanDeviceDummy> can_device;
00051   can_device = boost::dynamic_pointer_cast<icl_hardware::can::tCanDeviceDummy>(my_controller.getCanDevice());
00052 
00053   // Add one node
00054   my_controller.addGroup<DS301Group>("testgroup");
00055   my_controller.addNode<DS301Node>(1, "testgroup");
00056   DS301Group::Ptr my_group = my_controller.getGroup<DS301Group>("testgroup");
00057   DS301Node::Ptr node = my_group->getNodes().front();
00058 
00059 
00060   // Create some data that should be downloaded to the slave node
00061   std::vector<uint8_t> data(4);
00062   data[0] = 0x00;
00063   data[1] = 0x01;
00064   data[2] = 0x02;
00065   data[3] = 0x03;
00066   uint32_t index = 300;
00067   uint8_t subindex = 3;
00068 
00069   // create node response
00070   CanMsg response;
00071   response.id = ds301::ID_TSDO_MIN; // SDO for first node
00072   response.dlc = 8;
00073   response.rtr = 0;
00074   response.data[0] = SDO::SDO_SEG_RES_INIT_DOWNLOAD;
00075   response.data[1] = index & 0xff;
00076   response.data[2] = index >> 8;
00077   response.data[3] = subindex & 0xff;
00078   can_device->addResponse(response);
00079 
00080   // If download succeeds and the response is correct, this will pass
00081   BOOST_REQUIRE(node->m_sdo.download(false, index, subindex, data));
00082 
00083   // parse the sent can message that was created by the download request
00084   CanMsg msg = can_device->getLastMessage();
00085   LOGGING_INFO(CanOpen, hexArrayToString(msg.data, msg.dlc) << endl);
00086   BOOST_REQUIRE(msg.dlc == 8);
00087   BOOST_REQUIRE(msg.rtr == 0);
00088   BOOST_REQUIRE(msg.data[0] == SDO::SDO_SEG_REQ_INIT_DOWNLOAD_4BYTE);
00089   BOOST_REQUIRE(msg.data[1] == (index & 0xff));
00090   BOOST_REQUIRE(msg.data[2] == (index >> 8));
00091   BOOST_REQUIRE(msg.data[3] == subindex);
00092   BOOST_REQUIRE(msg.data[4] == data[0]);
00093   BOOST_REQUIRE(msg.data[5] == data[1]);
00094   BOOST_REQUIRE(msg.data[6] == data[2]);
00095   BOOST_REQUIRE(msg.data[7] == data[3]);
00096   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00097   std::cout << "\n";
00098 
00099   // Now let's generate some faulty responses. They should all fail.
00100   response.dlc = 13; // some arbitrary illegal number
00101   can_device->addResponse(response);
00102 
00103   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), std::exception);
00104   response.dlc = 8;
00105   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00106   std::cout << "\n";
00107 
00108 
00109   // This will produce a timeout as the message will never reach the SDO
00110   response.id = ds301::ID_TSDO_MIN-1;
00111   can_device->addResponse(response);
00112   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), TimeoutException);
00113   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00114   std::cout << "\n";
00115 
00116   // This will also produce a timout, as the node to this sdo does not exist
00117   response.id = ds301::ID_TSDO_MIN+1;
00118   can_device->addResponse(response);
00119   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), TimeoutException);
00120   response.id = ds301::ID_TSDO_MIN;
00121   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00122   std::cout << "\n";
00123 
00124   // This will produce a wrong response error
00125   response.data[0] = SDO::SDO_SEG_RES_INIT_DOWNLOAD | 0x01;
00126   can_device->addResponse(response);
00127   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), ResponseException);
00128   response.data[0] = SDO::SDO_SEG_RES_INIT_DOWNLOAD;
00129   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00130   std::cout << "\n";
00131 
00132   // LSB byte of index is wrong
00133   response.data[1] = (index+1) & 0xff;
00134   can_device->addResponse(response);
00135   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), ResponseException);
00136   response.data[1] = index & 0xff;
00137   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00138   std::cout << "\n";
00139 
00140   // MSB byte of index is wrong
00141   response.data[2] = (index+256) >> 8;
00142   can_device->addResponse(response);
00143   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), ResponseException);
00144   response.data[2] = index >> 8;
00145   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00146   std::cout << "\n";
00147 
00148   // Subindex is wrong
00149   response.data[3] = (subindex+1) & 0xff;
00150   can_device->addResponse(response);
00151   BOOST_REQUIRE_THROW(node->m_sdo.download(false, index, subindex, data), ResponseException);
00152   response.data[3] = subindex & 0xff;
00153   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00154   std::cout << "\n";
00155 
00156   // blocked downloads are not yet supported
00157   can_device->addResponse(response);
00158   try {
00159     BOOST_REQUIRE(!(node->m_sdo.download(true, index, subindex, data)));
00160   }
00161   catch (const std::exception& e)
00162   {
00163     LOGGING_ERROR_C (CanOpen, SDO, e.what() << endl);
00164   }
00165   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00166   std::cout << "\n";
00167 
00168   // illegal number of sent bytes
00169   can_device->addResponse(response);
00170   std::vector<uint8_t> empty_data;
00171   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, empty_data), ProtocolException);
00172   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00173   std::cout << "\n";
00174 
00175   // illegal number of sent bytes
00176   can_device->addResponse(response);
00177   empty_data.resize(5);
00178   try {
00179     BOOST_REQUIRE(!(node->m_sdo.download(false, index, subindex, empty_data)));
00180   }
00181   catch (const std::exception& e)
00182   {
00183     LOGGING_ERROR_C (CanOpen, SDO, e.what() << endl);
00184   }
00185   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00186   std::cout << "\n";
00187 }
00188 
00189 
00190 BOOST_AUTO_TEST_CASE (sdo_download_test_interfaces)
00191 {
00192   // Initializing
00193   icl_core::logging::initialize();
00194   icl_core::logging::setLogLevel(icl_core::logging::eLL_TRACE);
00195   LOGGING_INFO(CanOpen, "---------------------------------------------" << endl <<
00196                         "-----Running SDO download interface test-----" << endl <<
00197                         "---------------------------------------------" << endl);
00198 
00199   CanOpenController my_controller("Dummy");
00200   boost::shared_ptr<icl_hardware::can::tCanDeviceDummy> can_device;
00201   can_device = boost::dynamic_pointer_cast<icl_hardware::can::tCanDeviceDummy>(my_controller.getCanDevice());
00202 
00203   // Add one node
00204   my_controller.addGroup<DS301Group>("testgroup");
00205   my_controller.addNode<DS301Node>(1, "testgroup");
00206   DS301Group::Ptr my_group = my_controller.getGroup<DS301Group>("testgroup");
00207   DS301Node::Ptr node = my_group->getNodes().front();
00208 
00209 
00210   // Create some data that should be downloaded to the slave node
00211   std::vector<uint8_t> data(4);
00212   data[0] = 0x00;
00213   data[1] = 0x01;
00214   data[2] = 0x02;
00215   data[3] = 0x03;
00216   uint32_t index = 300;
00217   uint8_t subindex = 3;
00218 
00219   // create node response
00220   CanMsg response;
00221   response.id = ds301::ID_TSDO_MIN; // SDO for first node
00222   response.dlc = 8;
00223   response.rtr = 0;
00224   response.data[0] = SDO::SDO_SEG_RES_INIT_DOWNLOAD;
00225   response.data[1] = index & 0xff;
00226   response.data[2] = index >> 8;
00227   response.data[3] = subindex & 0xff;
00228   can_device->addResponse(response);
00229 
00230   BOOST_REQUIRE_NO_THROW(node->m_sdo.download(false, index, subindex, data));
00231   // parse the sent can message that was created by the download request
00232   CanMsg msg1 = can_device->getLastMessage();
00233 
00234   can_device->addResponse(response);
00235   uint32_t data_block = (data[3] << 24) + (data[2] << 16) + (data[1] << 8) + data[0];
00236   LOGGING_INFO (CanOpen, hexToString(data_block) << endl);
00237   BOOST_REQUIRE_NO_THROW(node->m_sdo.download(false, index, subindex, data_block));
00238 
00239   // parse the sent can message that was created by the download request
00240   CanMsg msg2 = can_device->getLastMessage();
00241   LOGGING_INFO(CanOpen, hexArrayToString(msg2.data, msg2.dlc) << endl);
00242 
00243   BOOST_REQUIRE(msg2.dlc == 8);
00244   BOOST_REQUIRE(msg2.rtr == 0);
00245   BOOST_REQUIRE(msg2.data[1] == (index & 0xff));
00246   BOOST_REQUIRE(msg2.data[0] == SDO::SDO_SEG_REQ_INIT_DOWNLOAD_4BYTE);
00247   BOOST_REQUIRE(msg2.data[2] == (index >> 8));
00248   BOOST_REQUIRE(msg2.data[3] == subindex);
00249   BOOST_REQUIRE(msg2.data[4] == data[0]);
00250   BOOST_REQUIRE(msg2.data[5] == data[1]);
00251   BOOST_REQUIRE(msg2.data[6] == data[2]);
00252   BOOST_REQUIRE(msg2.data[7] == data[3]);
00253 
00254   BOOST_REQUIRE(msg1.data[4] == msg2.data[4]);
00255   BOOST_REQUIRE(msg1.data[5] == msg2.data[5]);
00256   BOOST_REQUIRE(msg1.data[6] == msg2.data[6]);
00257   BOOST_REQUIRE(msg1.data[7] == msg2.data[7]);
00258 }
00259 
00260 BOOST_AUTO_TEST_CASE (sdo_upload)
00261 {
00262   // Initializing
00263   icl_core::logging::initialize();
00264   icl_core::logging::setLogLevel(icl_core::logging::eLL_TRACE);
00265   LOGGING_INFO(CanOpen, "---------------------------------" << endl <<
00266                         "-----Running SDO upload test-----" << endl <<
00267                         "---------------------------------" << endl);
00268 
00269   CanOpenController my_controller("Dummy");
00270   boost::shared_ptr<icl_hardware::can::tCanDeviceDummy> can_device;
00271   can_device = boost::dynamic_pointer_cast<icl_hardware::can::tCanDeviceDummy>(my_controller.getCanDevice());
00272 
00273   // Add one node
00274   my_controller.addGroup<DS301Group>("testgroup");
00275   my_controller.addNode<DS301Node>(1, "testgroup");
00276   DS301Group::Ptr my_group = my_controller.getGroup<DS301Group>("testgroup");
00277   DS301Node::Ptr node = my_group->getNodes().front();
00278 
00279 
00280   uint32_t index = 300;
00281   uint8_t subindex = 3;
00282 
00283   // This is the data that will be uploaded from the slave to the master.
00284   std::vector<uint8_t> data(4);
00285   data[0] = 0x01;
00286   data[1] = 0x02;
00287   data[2] = 0x03;
00288   data[3] = 0x04;
00289 
00290   // create node response
00291   CanMsg response;
00292   response.id = ds301::ID_TSDO_MIN; // SDO for first node
00293   response.dlc = 8;
00294   response.rtr = 0;
00295   response.data[0] = SDO::SDO_SEG_RES_INIT_UPLOAD_4BYTE;
00296   response.data[1] = index & 0xff;
00297   response.data[2] = index >> 8;
00298   response.data[3] = subindex & 0xff;
00299   response.data[4] = data[0];
00300   response.data[5] = data[1];
00301   response.data[6] = data[2];
00302   response.data[7] = data[3];
00303   can_device->addResponse(response);
00304 
00305   std::vector<uint8_t> uploaded_data;
00306 
00307   // If upload succeeds and the response is valid, this will pass
00308   try
00309   {
00310     BOOST_REQUIRE(node->m_sdo.upload(false, index, subindex, uploaded_data));
00311   }
00312   catch (const std::exception& e)
00313   {
00314     LOGGING_ERROR (CanOpen, e.what() << endl);
00315   }
00316 
00317   // did the node upload the correct data?
00318   BOOST_REQUIRE(data == uploaded_data);
00319 
00320   // parse the sent can message that was created by the upload request
00321   CanMsg msg = can_device->getLastMessage();
00322   LOGGING_INFO(CanOpen, hexArrayToString(msg.data, msg.dlc) << endl);
00323   BOOST_REQUIRE(msg.dlc == 8);
00324   BOOST_REQUIRE(msg.rtr == 0);
00325   BOOST_REQUIRE(msg.data[0] == SDO::SDO_SEG_REQ_INIT_UPLOAD);
00326   BOOST_REQUIRE(msg.data[1] == (index & 0xff));
00327   BOOST_REQUIRE(msg.data[2] == (index >> 8));
00328   BOOST_REQUIRE(msg.data[3] == subindex);
00329 
00330   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00331   std::cout << "\n";
00332 
00333   // Now let's generate some faulty responses. They should all fail.
00334   response.dlc = 13; // some arbitrary illegal number
00335   can_device->addResponse(response);
00336   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), TimeoutException);
00337   response.dlc = 8;
00338   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00339   std::cout << "\n";
00340 
00341 
00342   // This will produce a timeout as the message will never reach the SDO
00343   response.id = ds301::ID_TSDO_MIN-1;
00344   can_device->addResponse(response);
00345   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), TimeoutException);
00346   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00347   std::cout << "\n";
00348 
00349   // This will also produce a timout, as the node to this sdo does not exist
00350   response.id = ds301::ID_TSDO_MIN+1;
00351   can_device->addResponse(response);
00352   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), TimeoutException);
00353   response.id = ds301::ID_TSDO_MIN;
00354   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00355   std::cout << "\n";
00356 
00357   // This will produce a wrong response error
00358   response.data[0] = SDO::SDO_SEG_REQ_INIT_UPLOAD | 0x04;
00359   can_device->addResponse(response);
00360   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), ResponseException);
00361   response.data[0] = SDO::SDO_SEG_REQ_INIT_UPLOAD;
00362   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00363   std::cout << "\n";
00364 
00365   // LSB byte of index is wrong
00366   response.data[1] = (index+1) & 0xff;
00367   can_device->addResponse(response);
00368   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), ResponseException);
00369   response.data[1] = index & 0xff;
00370   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00371   std::cout << "\n";
00372 
00373   // MSB byte of index is wrong
00374   response.data[2] = (index+256) >> 8;
00375   can_device->addResponse(response);
00376   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), ResponseException);
00377   response.data[2] = index >> 8;
00378   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00379   std::cout << "\n";
00380 
00381   // Subindex is wrong
00382   response.data[3] = (subindex+1) & 0xff;
00383   can_device->addResponse(response);
00384   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), ResponseException);
00385   response.data[3] = subindex & 0xff;
00386   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00387   std::cout << "\n";
00388 
00389   // blocked uploads are not yet supported
00390   can_device->addResponse(response);
00391   BOOST_REQUIRE(!(node->m_sdo.upload(true, index, subindex, uploaded_data)));
00392 
00393   boost::this_thread::sleep(boost::posix_time::milliseconds(200));
00394   std::cout << "\n";
00395 
00396 
00397   // send a response with an error
00398   response.data[0] = 0x80; // abort transfer
00399   std::vector<uint8_t> char_vec = convertToCharVector(0x06010000); // Unsupported access to an object
00400   response.data[4] = char_vec[0];
00401   response.data[5] = char_vec[1];
00402   response.data[6] = char_vec[2];
00403   response.data[7] = char_vec[3];
00404   can_device->addResponse(response);
00405   BOOST_REQUIRE_THROW (node->m_sdo.download(false, index, subindex, uploaded_data), ProtocolException);
00406   boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00407   std::cout << "\n";
00408 }
00409 
00410 BOOST_AUTO_TEST_SUITE_END()


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Sun May 22 2016 03:30:56