LdmrsFieldApp.cpp
Go to the documentation of this file.
00001 //
00002 // LdmrsFieldApp.cpp
00003 //
00004 // Demo application.
00005 //
00006 
00007 #include "LdmrsFieldApp.hpp"
00008 #include "../tools/errorhandler.hpp"    // for printInfoMessage()
00009 #include "../tools/toolbox.hpp"                 // for toString()
00010 #include "../datatypes/Scan.hpp"
00011 #include "../datatypes/Object.hpp"
00012 #include "../datatypes/Msg.hpp"
00013 #include "../datatypes/Measurement.hpp"
00014 #include "../datatypes/Fields.hpp"
00015 #include "../datatypes/EvalCases.hpp"
00016 #include "../datatypes/EvalCaseResults.hpp"
00017 #include "../devices/LD_MRS.hpp"
00018 
00019 namespace application
00020 {
00021 
00022 //
00023 // Constructor
00024 //
00025 LdmrsFieldApp::LdmrsFieldApp(Manager* manager)
00026 {
00027         m_beVerbose = true;
00028         m_manager = manager;
00029         
00030         // Start the thread
00031         if (m_changeThread.isRunning() == false)
00032         {
00033                 m_changeThread.run(this);
00034         }
00035 
00036 
00037         printInfoMessage("LdmrsFieldApp constructor done.", m_beVerbose);
00038 }
00039 
00040 
00041 // Destructor
00042 // Clean up all dynamic data structures
00043 LdmrsFieldApp::~LdmrsFieldApp()
00044 {
00045         printInfoMessage("LdmrsFieldApp says Goodbye!", m_beVerbose);
00046 }
00047 
00048 
00049 //
00050 // Receiver for new data from the manager.
00051 //
00052 void LdmrsFieldApp::setData(BasicData& data)
00053 {
00054         //
00055         // Do something with it.
00056         //
00057         // Here, we just print what we've got.
00058         //
00059         std::string datatypeStr;
00060         std::string sourceIdStr;
00061 
00062         switch (data.getDatatype())
00063         {
00064                 case Datatype_Scan:
00065                         datatypeStr = "Scan (" + ::toString(((Scan&)data).getNumPoints()) + " points)";
00066                         {
00067                                 // Print the scan start timestamp (NTP time) 
00068                                 Scan* scan = dynamic_cast<Scan*>(&data);
00069                                 const ScannerInfo* info = scan->getScannerInfoByDeviceId(1);
00070  
00071                                 if (info != NULL)
00072                                 {
00073                                         const Time& time = info->getStartTimestamp();
00074                                         printInfoMessage("LdmrsApp::setData(): Scan start time: " + time.toString(), m_beVerbose);
00075                                 }
00076                         }
00077                         break;
00078                 case Datatype_Objects:
00079                         datatypeStr = "Objects (" + ::toString(((ObjectList&)data).size()) + " objects)";
00080                         break;
00081                 case Datatype_Fields:
00082                         datatypeStr = "Fields (" + ::toString(((Fields&)data).getFields().size()) + " fields, " +
00083                                                         ::toString(((Fields&)data).getNumberOfValidFields()) + " of which are valid)";
00084                         break;
00085                 case Datatype_EvalCases:
00086                         datatypeStr = "EvalCases (" + ::toString(((EvalCases&)data).getEvalCases().size()) + " cases)";
00087                         break;
00088                 case Datatype_EvalCaseResults:
00089                         datatypeStr = "EvalCaseResults (" + ::toString(((EvalCaseResults&)data).size()) + " case results)";
00090                         break;
00091                 case Datatype_Msg:
00092                         datatypeStr = "Msg (" + ((Msg&)data).toString() + ")";
00093                         break;
00094                 case Datatype_MeasurementList:
00095                         datatypeStr = "MeasurementList (" + ::toString(((MeasurementList&)data).m_list.size()) +" entries)";
00096                         break;
00097                 default:
00098                         datatypeStr = "(unknown)";
00099         }
00100         
00101         sourceIdStr = ::toString(data.getSourceId());
00102         
00103         printInfoMessage("LdmrsApp::setData(): Called with data of type " + datatypeStr + " from ID " + sourceIdStr + ".", m_beVerbose);
00104 }
00105 
00106 
00107 //
00108 // Demo function that removes all fields by overwriting them with segmented fields,
00109 // and setting them to invalid.
00110 //
00111 void LdmrsFieldApp::thread_removeAllEvalCases(devices::LDMRS* ldmrs)
00112 {
00113         bool beVerboseHere = m_beVerbose;
00114         beVerboseHere = true;
00115         
00116         printInfoMessage("LdmrsFieldApp::thread_removeAllEvalCases: Called.", beVerboseHere);
00117 
00118         bool result;
00119         EvalCases cases;
00120         result = ldmrs->writeEvalCases(cases);
00121         if (result == false)
00122         {
00123                 // Failure
00124                 printError("LdmrsFieldApp::thread_removeAllEvalCases: Failed to write empty EvalCase structure!");
00125         }
00126 
00127         printInfoMessage("LdmrsFieldApp::thread_removeAllEvalCases: All done, leaving.", beVerboseHere);
00128 }
00129 
00130 
00131 //
00132 // Demo function that removes all fields by overwriting them with segmented fields,
00133 // and setting them to invalid.
00134 //
00135 void LdmrsFieldApp::thread_removeAllFields(devices::LDMRS* ldmrs)
00136 {
00137         bool beVerboseHere = m_beVerbose;
00138         beVerboseHere = true;
00139         
00140         printInfoMessage("LdmrsFieldApp::thread_removeAllFields: Called.", beVerboseHere);
00141 
00142         bool result;
00143         
00144         // Create an empty field which is invalid
00145         FieldParameter field;
00146         field.setDistScaleFactor(10);   // With a value of 10, coordinate unit is [cm]
00147         field.setDistScaleOffset(0);    // No offset
00148         field.setAngleScaleFactor(8);   // Angle unit is [1/4 deg]
00149         field.setAngleScaleOffset(-1920);       // -1920 = -60 deg (=60 deg due to other coordinate system)
00150         field.setComment("");
00151         field.setFieldName("");
00152         field.setEnableLayerFilter(false);
00153         field.setVersionNumber(1);              // Version number is 1
00154         field.setFieldTypeIntern(FieldParameter::FieldTypeIntern_SEGMENTED);    // Segmented = 2
00155         field.setFieldNumber(0);                // 0 is an invalid field
00156         field.setField(NULL);                   // (FieldDescription*)
00157         
00158         for (UINT16 i = 0; i<16; i++)
00159         {
00160                 printInfoMessage("LdmrsFieldApp::thread_removeAllFields: Removing field " + toString(i) + ".", beVerboseHere);
00161                 result = ldmrs->writeField(i, field);
00162                 if (result == false)
00163                 {
00164                         // Failure
00165                         printError("LdmrsFieldApp::thread_removeAllFields: Failed to clear field " + toString(i) + "!");
00166                 }
00167         }
00168         
00169         printInfoMessage("LdmrsFieldApp::thread_removeAllFields: All done, leaving.", beVerboseHere);
00170 }
00171 
00172 //
00173 // Demo function that creates an eval case.
00174 //
00175 void LdmrsFieldApp::thread_createEvalCase(devices::LDMRS* ldmrs)
00176 {
00177         bool beVerboseHere = m_beVerbose;
00178         beVerboseHere = true;
00179         
00180         printInfoMessage("LdmrsFieldApp::thread_createEvalCase(): Called.", beVerboseHere);
00181 
00182         bool result;
00183         
00184         // The single eval case
00185         EvalCase* c = new EvalCase;
00186         c->setVersionNumber(1);
00187         c->setCaseNumber(1);                                    // Case number must be >0 for a valid case
00188         c->setStrategy(EvalCase::BLANKING);
00189         c->setResultNegation(false);
00190         c->setResponseTime(500);                                // ms
00191         c->setResponseTimeExtended(500);                // ms
00192         c->setOutputNumber(0);                          // 0 = none
00193         c->setLogicalInputState_from_UINT8(0x00);       // 0 = no inputs
00194         c->setDistDependent(false);
00195         c->setMaxRadialCorridor(2.0);
00196         c->setManipulationPrevention(EvalCase::ECS_INACTIVE);
00197         c->setBlankingSize(0.2);
00198         c->setMinFieldExp(0);
00199         c->setFieldNumber(1);
00200         c->setFilterType(EvalCase::UNFILTERED);
00201         c->setCaseName("DemoCase-1");
00202         c->setComment("");
00203         
00204         // The cases carrier structure
00205         EvalCases cases;
00206         cases.add(c);
00207         
00208         result = ldmrs->writeEvalCases(cases);
00209         if (result == false)
00210         {
00211                 // Failure
00212                 printError("LdmrsFieldApp::thread_createEvalCase: Failed to write the EvalCase structure!");
00213         }
00214 
00215         printInfoMessage("LdmrsFieldApp::thread_createEvalCase: All done, leaving.", beVerboseHere);
00216 }
00217 
00218 //
00219 // Demo function that creates a rectangular field.
00220 //
00221 void LdmrsFieldApp::thread_createRectangularField(devices::LDMRS* ldmrs)
00222 {
00223         bool beVerboseHere = m_beVerbose;
00224         beVerboseHere = true;
00225         
00226         printInfoMessage("LdmrsFieldApp::thread_createRectangularField(): Called.", beVerboseHere);
00227 
00228         // Create an empty field which is invalid
00229         FieldParameter field;
00230         field.setDistScaleFactor(10);   // With a value of 10, coordinate unit is [cm]
00231         field.setDistScaleOffset(0);    // No offset
00232         field.setAngleScaleFactor(8);   // Angle unit is [1/4 deg]
00233         field.setAngleScaleOffset(-1920);       // -1920 = -60 deg (=60 deg due to other coordinate system)
00234         field.setComment("");
00235         field.setFieldName("Rect_1");
00236         field.setEnableLayerFilter(false);
00237         field.setVersionNumber(1);              // Version number is 1
00238         field.setFieldTypeIntern(FieldParameter::FieldTypeIntern_RECTANGLE);    // Rectangle = 1
00239         field.setFieldNumber(1);                // A field number 0 marks the field as invalid
00240         
00241         // Create a rectangular field sub-structure
00242         FieldRectangle rectField;
00243 //      FieldSegmentedPoint segPt;
00244 
00245         rectField.setRefPointDist(2.06);        // Dist to ref point is 2.06 m
00246         rectField.setRefPointAngle(13.8 * deg2rad);     // Angle of 13.8 deg
00247         rectField.setRotAngle(0.0 * deg2rad);                   // 0 = No rotation
00248         rectField.setLength(5.0);                       // Length 5 m (x direction)
00249         rectField.setWidth(2.0);                        // Width 2 m (y direction)
00250         
00251         // Add the rectangular field to the field
00252         field.setField(&rectField);     // (FieldDescription*)
00253 
00254         // Write the field as field 1 to the sensor
00255         bool result = false;
00256         result = ldmrs->writeField(0, field);
00257         if (result == false)
00258         {
00259                 // Failure
00260                 printError("LdmrsFieldApp::thread_createRectangularField(): Failed to write the field!");
00261         }
00262         
00263         printInfoMessage("LdmrsFieldApp::thread_createRectangularField(): All done, leaving.", beVerboseHere);
00264 }
00265 
00266 //
00267 // Thread that does the actual changing of parameters.
00268 //
00269 void LdmrsFieldApp::changeThreadFunction(bool& endThread, UINT16& waitTimeMs)
00270 {
00271         bool beVerboseHere = m_beVerbose;
00272         beVerboseHere = true;
00273                 
00274         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): started", beVerboseHere);
00275 
00276         devices::LDMRS* ldmrs;
00277         
00278         // Wait until the LD-MRS is present
00279         for (UINT16 i = 0; i<10; i++)
00280         {
00281                 ldmrs = dynamic_cast<devices::LDMRS*>(m_manager->getFirstDeviceByType(Sourcetype_LDMRS));
00282                 if (ldmrs == NULL)
00283                 {
00284                         // Sleep for a second
00285                         usleep(1000000);
00286                 }
00287         }
00288 
00289         if (ldmrs == NULL)
00290         {
00291                 // Scanner device not found
00292                 printError("LdmrsFieldApp::changeThreadFunction(): Failed to read device pointer, aborting!");
00293                 endThread = true;
00294                 waitTimeMs = 0;
00295                 return;
00296         }
00297         
00298         // Now wait until the connection is established
00299         for (UINT16 i = 0; i<10; i++)
00300         {
00301                 if (ldmrs->isRunning() == false)
00302                 {
00303                         // Sleep for a second
00304                         usleep(1000000);
00305                 }
00306         }
00307         // Sleep some more, just to be sure
00308         usleep(3 * 1000000);
00309         
00310         UINT32 sleepTimeS;
00311 
00312         
00313         //
00314         // Remove all existing eval cases
00315         //
00316         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now removing all eval cases.", beVerboseHere);
00317         thread_removeAllEvalCases(ldmrs);
00318         
00319 
00320         //
00321         // Remove all exisiting fields by overwriting them with invalid fields.
00322         //
00323         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now removing all fields.", beVerboseHere);
00324         thread_removeAllFields(ldmrs);
00325         
00326         // sleep some time
00327         sleepTimeS = 3;
00328         usleep(sleepTimeS * 1000 * 1000);
00329 
00330         
00331         //
00332         // Write a single rectangular field to the sensor.
00333         //
00334         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now writing a rectangular field.", beVerboseHere);
00335         thread_createRectangularField(ldmrs);
00336         
00337         // sleep some time
00338         sleepTimeS = 1;
00339         usleep(sleepTimeS * 1000 * 1000);
00340 
00341         
00342         //
00343         // Write a single eval case to the sensor.
00344         //
00345         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now writing an eval case for the rectangular field.", beVerboseHere);
00346         thread_createEvalCase(ldmrs);
00347         
00348         
00349         //
00350         // Save the current field configuration permanently.
00351         //
00352 //      printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now flashing the current field configuration.", beVerboseHere);
00353 //      ldmrs->flashSopasConfig();
00354         
00355         // sleep some time
00356         sleepTimeS = 5;
00357         usleep(sleepTimeS * 1000 * 1000);
00358 
00359         
00360         endThread = true;
00361         waitTimeMs = 0;
00362         printInfoMessage("LdmrsFieldApp::changeThreadFunction(): All done, leaving.", m_beVerbose);
00363 }
00364 
00365 
00366 }       // namespace application


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Thu Jun 6 2019 21:02:36