LD_MRS.cpp
Go to the documentation of this file.
00001 //
00002 // LD_MRS.cpp
00003 //
00004 // Device class of a simple LD-MRS laserscanner interface.
00005 //
00006 //
00007 // To configure the device, add the parameters in the constructor below.
00008 //
00009 // VERSION 1.1.0
00010 //
00011 // Version history:
00012 // 1.0.0  , willhvo
00013 //   - Initial version
00014 //
00015 // 1.1.0  , willhvo
00016 //   - Added reading from file
00017 //
00018 
00019 
00020 //
00021 #include "LD_MRS.hpp"
00022 #include "../tools/errorhandler.hpp"
00023 
00024 namespace devices
00025 {
00026 
00027 using namespace datatypes;
00028 
00029 LDMRS::LDMRS(Manager* manager)
00030         : m_manager(manager)
00031         , m_sopas(NULL)
00032         , m_lux(NULL)
00033 {
00034         setDevicetype(Sourcetype_LDMRS);
00035         m_beVerbose = false;
00036         
00037         printInfoMessage("LDMRS::LDMRS: Starting constructor.", m_beVerbose);
00038         
00039         m_lux = NULL;
00040         
00041         m_weWantScanData = true;        // Enable or disable scan data reception via the MRS (=non-SOPAS) interface.
00042         m_weWantObjectData = false;     // Enable or disable object data reception via the MRS (=non-SOPAS) interface.
00043         m_weWantFieldData = false;      // Enable or disable protection field and eval case data reception via the SOPAS interface.
00044         m_weWantScanDataFromSopas = false;      // Enable or disable scan data reception via the SOPAS interface.
00045 
00046 //      m_inputFileName = "/home/willhvo/Temp/raw_ldmrs_log.bin";               // File name of input file. If this name is given, connects to file instead of TCP.
00047         m_inputFileName = "";
00048         m_ipAddress = "192.168.0.1";    // , "Network IP address where the device is located.", "0.0.0.0");
00049         m_luxPortNumber = 12002;        // Network port on which the legacy (non-sopas) device is contacted (for scans and objects), typically 12002
00050         m_SopasPortNumber = 2111;       // Network port on which the device is contacted, typically 2111 or 2112.
00051 
00052         // parameters for LuxBase/Scans
00053         m_scanFrequency = 12.5;                         // Scan frequency (12.5, 25 or 50 Hz)
00054         m_scanStartAngle = 45.0 * deg2rad;      // Start angle of the scan.
00055         m_scanEndAngle = -45.0 * deg2rad;       // End angle of the scan.
00056         m_readOnlyMode = false;         // Forbids parameter writes to the scanner if true.
00057 
00058 //      m_scanAngleRes = 0.25;
00059 //      m_skipNumScans = 0;             // Use this value if the sensor scan frequency is too high and if scans should be skipped.
00060 
00061         m_offsetX = 0.0;        // x-coordinate of sensor mounting position in the vehicle coordinate system.
00062         m_offsetY = 0.0;        // y-coordinate of sensor mounting position in the vehicle coordinate system
00063         m_offsetZ = 0.0;        // z-coordinate of sensor mounting position in the vehicle coordinate system
00064         m_yawAngle = 0.0;       // Yaw angle of sensor mounting position in the vehicle coordinate system ("left and right")
00065         m_pitchAngle = 0.0;     // Pitch angle of sensor mounting position in the vehicle coordinate system ("up and down")
00066         m_rollAngle = 0.0;      // Roll angle of sensor mounting position in the vehicle coordinate system ("to the sides")
00067 
00068         printInfoMessage("LDMRS::LDMRS: Constructor done.", m_beVerbose);
00069 }
00070 
00071 
00072 
00073 LDMRS::~LDMRS()
00074 {
00075         printInfoMessage("LDMRS::~LDMRS: Starting destructor.", m_beVerbose);
00076         
00077         if (isRunning())
00078         {
00079                 stop();
00080         }
00081         
00082         printInfoMessage("LDMRS::~LDMRS: Destructor done.", m_beVerbose);
00083 }
00084 
00085 
00086 //
00087 // Initialising.
00088 //
00089 // It would be nicer if this function would be called with all the device parameters,
00090 // instead of setting them as module variables...
00091 //
00092 bool LDMRS::init()
00093 {
00094         printInfoMessage("LDMRS::init: Called.", m_beVerbose);
00095 
00096         std::string longName = "LD-MRS";
00097         bool result = true;
00098 
00099 //      m_beVerbose = false;
00100         m_isRunning = false;
00101         
00102         // Altes Objekt loeschen
00103         if (m_lux != NULL)
00104         {
00105                 m_lux->stop();
00106                 delete m_lux;
00107                 m_lux = NULL;
00108         }
00109 
00110         // initializing LUX part
00111         printInfoMessage("LDMRS::init: Creating new LuxBase object.", m_beVerbose);
00112         m_lux = new LuxBase(m_manager,
00113                                                         getSourceId(),
00114                                                         longName,
00115                                                         m_ipAddress,
00116                                                         m_luxPortNumber,
00117                                                         m_scanFrequency,
00118                                                         m_scanStartAngle,
00119                                                         m_scanEndAngle,
00120                                                         m_offsetX,
00121                                                         m_offsetY,
00122                                                         m_offsetZ,
00123                                                         m_yawAngle,
00124                                                         m_pitchAngle,
00125                                                         m_rollAngle,
00126                                                         m_beVerbose,
00127                                                         m_inputFileName
00128                                         );
00129         
00130         printInfoMessage("LDMRS::init: Initializing the LuxBase object.", m_beVerbose);
00131         if (m_inputFileName == "")
00132         {
00133                 // TCP
00134                 result = m_lux->initTcp(LDMRS::disconnectFunctionS, (void*)this);
00135         }
00136         else
00137         {
00138                 // File
00139                 result = m_lux->initFile(LDMRS::disconnectFunctionS, (void*)this);
00140         }
00141 
00142         if (result == true)
00143         {
00144                 // Success
00145                 printInfoMessage("LDMRS::init: LuxBase was successfully initialized.", m_beVerbose);
00146         }
00147         else
00148         {
00149                 // Error
00150                 printError("LDMRS::init: Failed to initialize LuxBase - Device init failed, aborting.");
00151                 return false;
00152         }
00153         
00154         // Set the current time
00155         Time t = Time::now();
00156         if (m_inputFileName == "")
00157         {
00158                 // TCP
00159                 result = m_lux->cmd_setNtpTimestamp((UINT32)(t.seconds() + Time::secondsFrom1900to1970), 0);
00160                 if (result == true)
00161                 {
00162                         // Success
00163                         printInfoMessage("LDMRS::init: NTP timestamp was successfully set.", m_beVerbose);
00164                 }
00165                 else
00166                 {
00167                         // Error
00168                         printError("LDMRS::init: Failed to set NTP timestamp - Device init failed, aborting.");
00169                         return false;
00170                 }
00171         }
00172 
00173 
00174         return true;
00175 }
00176 
00177 
00178 //
00179 // Static-Einstiegspunkt fuer die DisconnectFunction.
00180 //
00181 void LDMRS::disconnectFunctionS(void* obj)
00182 {
00183         printInfoMessage("LDMRS::disconnectFunctionS: Called.", true);
00184 
00185         if (obj != NULL)
00186         {
00187                 ((LDMRS*)obj)->disconnectFunction();
00188         }
00189         
00190 //      SystemStateWatchdog::setNotConnected();
00191 
00192         printInfoMessage("LDMRS::disconnectFunctionS: Done.", true);
00193 }
00194 
00195 //
00196 //
00197 //
00198 void LDMRS::disconnectFunction()
00199 {
00200         printInfoMessage("LDMRS::disconnectFunction: LDMRS " + getDeviceName() + " was disconnected.", m_beVerbose);
00201 
00202 //      SystemStateWatchdog::setNotConnected();
00203 }
00204 
00205 
00206 //
00207 // Schliesse alle Schnittstellen.
00208 //
00209 void LDMRS::shutdown()
00210 {
00211         printInfoMessage("LDMRS::shutdown: called. Shutting down the LDMRS" + getDeviceName() + ".", m_beVerbose);
00212 
00213         if (m_lux->isRunning())
00214         {
00215                 stop();
00216         }
00217 }
00218 
00219 //
00220 // Write a field to the sensor
00221 //
00222 bool LDMRS::writeField(UINT16 fieldNum, const FieldParameter& para)
00223 {
00224         bool result;
00225         bool beVerboseHere = m_beVerbose;
00226         beVerboseHere = true;
00227         
00228         printInfoMessage("LDMRS::writeField: Called. Logging in now.", beVerboseHere);
00229         result = m_sopas->action_login();
00230         
00231         if (result == true)
00232         {
00233                 printInfoMessage("LDMRS::writeField: Login was successful, writing field now.", beVerboseHere);
00234                 result = m_sopas->action_writeField(fieldNum, para);
00235         }
00236         
00237         if (result == true)
00238         {
00239                 printInfoMessage("LDMRS::writeField: Field was written, logging out now.", beVerboseHere);
00240                 result = m_sopas->action_logout();
00241         }
00242         
00243         printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
00244         return result;
00245 }
00246 
00247 //
00248 // Write an EvalCase configuration to the sensor.
00249 // This is a structure with all (up to 16) eval cases.
00250 //
00251 bool LDMRS::writeEvalCases(const EvalCases& evalCases)
00252 {
00253         bool result;
00254         bool beVerboseHere = m_beVerbose;
00255         beVerboseHere = true;
00256         
00257         printInfoMessage("LDMRS::writeEvalCases: Called. Logging in now.", beVerboseHere);
00258         result = m_sopas->action_login();
00259         
00260         if (result == true)
00261         {
00262                 printInfoMessage("LDMRS::writeEvalCases: Login was successful, writing eval cases now.", beVerboseHere);
00263                 result = m_sopas->action_writeEvalCases(evalCases);
00264         }
00265         
00266         if (result == true)
00267         {
00268                 printInfoMessage("LDMRS::writeEvalCases: Eval cases were written, logging out now.", beVerboseHere);
00269                 result = m_sopas->action_logout();
00270         }
00271         
00272         printInfoMessage("LDMRS::writeEvalCases: All done, leaving.", beVerboseHere);
00273         return result;
00274 }
00275 
00276 //
00277 // Stores the SOPAS config data (fields and eval cases) permanently.
00278 //
00279 bool LDMRS::flashSopasConfig()
00280 {
00281         bool result;
00282         bool beVerboseHere = m_beVerbose;
00283         beVerboseHere = true;
00284         
00285         printInfoMessage("LDMRS::flashSopasConfig: Called.", beVerboseHere);
00286 
00287         result = m_sopas->action_flashFieldParameters();
00288         if (result == true)
00289         {
00290                 printInfoMessage("LDMRS::flashSopasConfig: Configuration was saved.", beVerboseHere);
00291         }
00292         else
00293         {
00294                 // Failure
00295                 printError("LDMRS::flashSopasConfig: Failed to save configuration!");
00296         }
00297         
00298         printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
00299         return result;
00300 }
00301 
00302 
00303 //
00304 // Sets the MRS-internal clock to the given time.
00305 //
00306 bool LDMRS::setNtpTime(UINT32 seconds, UINT32 fractionalSec)
00307 {
00308         bool result;
00309         bool beVerboseHere = m_beVerbose;
00310         beVerboseHere = true;
00311         
00312         printInfoMessage("LDMRS::setNtpTime: Called.", beVerboseHere);
00313         
00314         if (m_lux == NULL)
00315         {
00316                 printError("LDMRS::setNtpTime: No LUX-Base object available, aborting!");
00317                 return false;
00318         }
00319 
00320         result = m_lux->cmd_setNtpTimestamp(seconds, fractionalSec);
00321         if (result == true)
00322         {
00323                 printInfoMessage("LDMRS::setNtpTime: Timestamp was set.", beVerboseHere);
00324         }
00325         else
00326         {
00327                 // Failure
00328                 printError("LDMRS::setNtpTime: Failed to set timestamp!");
00329         }
00330         
00331         printInfoMessage("LDMRS::setNtpTime: All done, leaving.", beVerboseHere);
00332         return result;
00333 }
00334 
00335 bool LDMRS::setScanAngles(double startAngle, double endAngle)
00336 {
00337         bool result;
00338         bool beVerboseHere = m_beVerbose;
00339 
00340         printInfoMessage("LDMRS::setScanAngles: Called.", beVerboseHere);
00341 
00342         if (m_lux == NULL)
00343         {
00344                 printError("LDMRS::setScanAngles: No LUX-Base object available, aborting!");
00345                 return false;
00346         }
00347 
00348         result = m_lux->cmd_setScanAngles(startAngle, endAngle);
00349         if (result == true)
00350         {
00351                 printInfoMessage("LDMRS::setScanAngles: scan angles were set.", beVerboseHere);
00352         }
00353         else
00354         {
00355                 // Failure
00356                 printError("LDMRS::setScanAngles: Failed to set scan angles!");
00357         }
00358 
00359         printInfoMessage("LDMRS::setScanAngles: All done, leaving.", beVerboseHere);
00360         return result;
00361 }
00362 
00363 bool LDMRS::setSyncAngleOffset(double syncAngle)
00364 {
00365         bool result;
00366         bool beVerboseHere = m_beVerbose;
00367 
00368         printInfoMessage("LDMRS::setSyncAngleOffset: Called.", beVerboseHere);
00369 
00370         if (m_lux == NULL)
00371         {
00372                 printError("LDMRS::setSyncAngleOffset: No LUX-Base object available, aborting!");
00373                 return false;
00374         }
00375 
00376         result = m_lux->cmd_setSyncAngleOffset(syncAngle);
00377         if (result == true)
00378         {
00379                 printInfoMessage("LDMRS::setSyncAngleOffset: sync angle offset was set.", beVerboseHere);
00380         }
00381         else
00382         {
00383                 // Failure
00384                 printError("LDMRS::setSyncAngleOffset: Failed to set sync angle offset!");
00385         }
00386 
00387         printInfoMessage("LDMRS::setSyncAngleOffset: All done, leaving.", beVerboseHere);
00388         return result;
00389 }
00390 
00391 bool LDMRS::setScanFrequency(double scanFreq)
00392 {
00393         bool result;
00394         bool beVerboseHere = m_beVerbose;
00395 
00396         printInfoMessage("LDMRS::setScanFrequency: Called.", beVerboseHere);
00397 
00398         if (m_lux == NULL)
00399         {
00400                 printError("LDMRS::setScanFrequency: No LUX-Base object available, aborting!");
00401                 return false;
00402         }
00403 
00404         result = m_lux->cmd_setScanFrequency(scanFreq);
00405         if (result == true)
00406         {
00407                 printInfoMessage("LDMRS::setScanFrequency: scan frequency was set.", beVerboseHere);
00408         }
00409         else
00410         {
00411                 // Failure
00412                 printError("LDMRS::setScanFrequency: Failed to set scan frequency!");
00413         }
00414 
00415         printInfoMessage("LDMRS::setScanFrequency: All done, leaving.", beVerboseHere);
00416         return result;
00417 }
00418 
00419 std::string LDMRS::getIpAddress()
00420 {
00421         return m_ipAddress;
00422 }
00423 
00424 void LDMRS::setIpAddress(std::string ipAdress)
00425 {
00426         m_ipAddress = ipAdress;
00427 }
00428 
00429 void LDMRS::setWeWantObjectData(bool weWantObjectData) {
00430         m_weWantObjectData = weWantObjectData;
00431 }
00432 
00433 std::string LDMRS::getSerialNumber()
00434 {
00435         if (m_lux == NULL)
00436         {
00437                 return "(none)";
00438         } else {
00439                 return m_lux->getSerialNumber();
00440         }
00441 }
00442 
00443 std::string LDMRS::getFirmwareVersion()
00444 {
00445         if (m_lux == NULL)
00446         {
00447                 return "(none)";
00448         } else {
00449                 return m_lux->getFirmwareVersion();
00450         }
00451 }
00452 
00453 
00454 //
00455 // Starte das Einlesen von Daten (Scans, Objekte, Schutzfelder, ...)
00456 //
00457 bool LDMRS::run()
00458 {
00459         if (m_lux == NULL)
00460         {
00461                 printError("LDMRS::run: called, but pointer to internal data handler is NULL. Was init() called? Aborting!");
00462                 return false;
00463         }
00464         
00465         //
00466         // LUX-Part
00467         //
00468         printInfoMessage("LDMRS::run: Called. Run the LDMRS" + getDeviceName() + ".", m_beVerbose);
00469 
00470         // This will cause that we get scan and/or obect data via lux protocol
00471         bool luxSuccess = m_lux->run(m_weWantScanData, m_weWantObjectData);
00472         if (luxSuccess == true)
00473         {
00474                 // Success
00475                 printInfoMessage("LDMRS::run: Device " + getDeviceName() + " is running.", m_beVerbose);
00476                 m_lux->setOnScanReceiveCallbackFunction(LDMRS::onScanReceivedS, (void*)this);
00477         }
00478         else
00479         {
00480                 // Fail
00481                 printError("LDMRS::run: Failed to run device " + getDeviceName() + ", aborting!");
00482                 return false;
00483         }
00484 
00485         //
00486         // SOPAS part
00487         //
00488         if ((m_inputFileName == "") &&
00489                 ((m_weWantFieldData == true) ||
00490                  (m_weWantScanDataFromSopas == true)))
00491         {
00492                 // TCP only
00493                 bool sopasSuccess = true;
00494                 printInfoMessage("LDMRS::run: Now starting SOPAS part (eval cases) of device " + getDeviceName() + ".", m_beVerbose);
00495 
00496                 // Initialize Sopas part
00497                 m_sopas = new LdmrsSopasLayer(m_manager,
00498                                                                         getSourceId(),
00499                                                                         m_ipAddress,
00500                                                                         m_SopasPortNumber,
00501                                                                         m_weWantFieldData,
00502                                                                         m_weWantScanDataFromSopas,
00503                                                                         m_readOnlyMode);
00504                 sopasSuccess = m_sopas->init(LDMRS::disconnectFunctionS, (void*)this);
00505 
00506                 if (sopasSuccess == false)
00507                 {
00508                         printError("LDMRS::run: Failed to initialize SOPAS part of device " + getDeviceName() + ", aborting!");
00509                         stop();
00510                         return false;
00511                 }
00512 
00513                 // Success, now run the device
00514                 printInfoMessage("LDMRS::run: SOPAS part is initialized, now running it.", m_beVerbose);
00515                 sopasSuccess = m_sopas->run();
00516                 if (sopasSuccess == false)
00517                 {
00518                         printError("LDMRS::run: Failed to run SOPAS part of device " + getDeviceName() + ", aborting!");
00519                         stop();
00520                         return false;
00521                 }
00522                         
00523                 printInfoMessage("LDMRS::run: SOPAS part is running, device " + getDeviceName() + " is running, all done.", m_beVerbose);
00524         }
00525         
00526         return true;
00527 }
00528 
00529 
00530 //
00531 // Stop data reception, but do not shut down the connection.
00532 //
00533 bool LDMRS::stop()
00534 {
00535         printInfoMessage("LDMRS::stop: Called. Stopping the LDMRS" + getDeviceName() + ".", m_beVerbose);
00536         
00537         bool luxSuccess = true;
00538         bool sopasSuccess = true;
00539         
00540         if (m_sopas != NULL)
00541         {
00542                 if (m_sopas->isFieldDataSubscribed())
00543                 {
00544                         sopasSuccess = m_sopas->action_unSubscribeEvalCaseResults();
00545                 }
00546         }
00547 
00548         if (m_lux != NULL)
00549         {
00550                 luxSuccess = m_lux->stop();
00551         }
00552         
00553         return (luxSuccess && sopasSuccess);
00554 }
00555 
00556 
00557 bool LDMRS::isRunning()
00558 {
00559         if (m_lux == NULL) // no m_lux object yet? Is not running.
00560         {
00561                 return false;
00562         }
00563         bool luxSuccess = m_lux->isRunning();
00564         //bool sopasSuccess = m_sopas.isRunning();
00565         return (luxSuccess);    //  && sopasSuccess
00566 }
00567 
00568 //
00569 // Statischer Einstiegspunkt
00570 //
00571 void LDMRS::onScanReceivedS(void* obj)
00572 {
00573         if (obj != NULL)
00574         {
00575                 ((LDMRS*)obj)->onScanReceived();
00576         }
00577 }
00578 
00579 // after / before every scan we have to post the current sensor state as SensorInfo Structure
00580 void LDMRS::onScanReceived()
00581 {
00582 //      printInfoMessage("LDMRS::onScanReceived: Called.", m_beVerbose);
00583         
00584 //      SensorStateInfo sensorInfo = m_sopas.getSensorStateInfo();
00585 //      getSerializer().notifyMessageHandlers(sensorInfo, getDeviceID());
00586 }
00587 
00588 bool LDMRS::getParameter(MrsParameterId id, UINT32* value)
00589 {
00590         return m_lux->cmd_getParameter(id, value);
00591 }
00592 
00593 bool LDMRS::setParameter(MrsParameterId id, UINT32 value)
00594 {
00595         return m_lux->cmd_setParameter(id, value);
00596 }
00597 
00598 
00599 } // namespace devices
00600 


libsick_ldmrs
Author(s): SICK AG , Martin G√ľnther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50