LdmrsFieldApp.cpp
Go to the documentation of this file.
1 //
2 // LdmrsFieldApp.cpp
3 //
4 // Demo application.
5 //
6 
7 #include "LdmrsFieldApp.hpp"
8 #include "../tools/errorhandler.hpp" // for printInfoMessage()
9 #include "../tools/toolbox.hpp" // for toString()
10 #include "../datatypes/Scan.hpp"
11 #include "../datatypes/Object.hpp"
12 #include "../datatypes/Msg.hpp"
13 #include "../datatypes/Measurement.hpp"
14 #include "../datatypes/Fields.hpp"
15 #include "../datatypes/EvalCases.hpp"
16 #include "../datatypes/EvalCaseResults.hpp"
17 #include "../devices/LD_MRS.hpp"
18 
19 namespace application
20 {
21 
22 //
23 // Constructor
24 //
26 {
27  m_beVerbose = true;
28  m_manager = manager;
29 
30  // Start the thread
31  if (m_changeThread.isRunning() == false)
32  {
33  m_changeThread.run(this);
34  }
35 
36 
37  printInfoMessage("LdmrsFieldApp constructor done.", m_beVerbose);
38 }
39 
40 
41 // Destructor
42 // Clean up all dynamic data structures
44 {
45  printInfoMessage("LdmrsFieldApp says Goodbye!", m_beVerbose);
46 }
47 
48 
49 //
50 // Receiver for new data from the manager.
51 //
53 {
54  //
55  // Do something with it.
56  //
57  // Here, we just print what we've got.
58  //
59  std::string datatypeStr;
60  std::string sourceIdStr;
61 
62  switch (data.getDatatype())
63  {
64  case Datatype_Scan:
65  datatypeStr = "Scan (" + ::toString(((Scan&)data).getNumPoints()) + " points)";
66  {
67  // Print the scan start timestamp (NTP time)
68  Scan* scan = dynamic_cast<Scan*>(&data);
69  const ScannerInfo* info = scan->getScannerInfoByDeviceId(1);
70 
71  if (info != NULL)
72  {
73  const Time& time = info->getStartTimestamp();
74  printInfoMessage("LdmrsApp::setData(): Scan start time: " + time.toString(), m_beVerbose);
75  }
76  }
77  break;
78  case Datatype_Objects:
79  datatypeStr = "Objects (" + ::toString(((ObjectList&)data).size()) + " objects)";
80  break;
81  case Datatype_Fields:
82  datatypeStr = "Fields (" + ::toString(((Fields&)data).getFields().size()) + " fields, " +
83  ::toString(((Fields&)data).getNumberOfValidFields()) + " of which are valid)";
84  break;
85  case Datatype_EvalCases:
86  datatypeStr = "EvalCases (" + ::toString(((EvalCases&)data).getEvalCases().size()) + " cases)";
87  break;
89  datatypeStr = "EvalCaseResults (" + ::toString(((EvalCaseResults&)data).size()) + " case results)";
90  break;
91  case Datatype_Msg:
92  datatypeStr = "Msg (" + ((Msg&)data).toString() + ")";
93  break;
95  datatypeStr = "MeasurementList (" + ::toString(((MeasurementList&)data).m_list.size()) +" entries)";
96  break;
97  default:
98  datatypeStr = "(unknown)";
99  }
100 
101  sourceIdStr = ::toString(data.getSourceId());
102 
103  printInfoMessage("LdmrsApp::setData(): Called with data of type " + datatypeStr + " from ID " + sourceIdStr + ".", m_beVerbose);
104 }
105 
106 
107 //
108 // Demo function that removes all fields by overwriting them with segmented fields,
109 // and setting them to invalid.
110 //
112 {
113  bool beVerboseHere = m_beVerbose;
114  beVerboseHere = true;
115 
116  printInfoMessage("LdmrsFieldApp::thread_removeAllEvalCases: Called.", beVerboseHere);
117 
118  bool result;
119  EvalCases cases;
120  result = ldmrs->writeEvalCases(cases);
121  if (result == false)
122  {
123  // Failure
124  printError("LdmrsFieldApp::thread_removeAllEvalCases: Failed to write empty EvalCase structure!");
125  }
126 
127  printInfoMessage("LdmrsFieldApp::thread_removeAllEvalCases: All done, leaving.", beVerboseHere);
128 }
129 
130 
131 //
132 // Demo function that removes all fields by overwriting them with segmented fields,
133 // and setting them to invalid.
134 //
136 {
137  bool beVerboseHere = m_beVerbose;
138  beVerboseHere = true;
139 
140  printInfoMessage("LdmrsFieldApp::thread_removeAllFields: Called.", beVerboseHere);
141 
142  bool result;
143 
144  // Create an empty field which is invalid
145  FieldParameter field;
146  field.setDistScaleFactor(10); // With a value of 10, coordinate unit is [cm]
147  field.setDistScaleOffset(0); // No offset
148  field.setAngleScaleFactor(8); // Angle unit is [1/4 deg]
149  field.setAngleScaleOffset(-1920); // -1920 = -60 deg (=60 deg due to other coordinate system)
150  field.setComment("");
151  field.setFieldName("");
152  field.setEnableLayerFilter(false);
153  field.setVersionNumber(1); // Version number is 1
155  field.setFieldNumber(0); // 0 is an invalid field
156  field.setField(NULL); // (FieldDescription*)
157 
158  for (UINT16 i = 0; i<16; i++)
159  {
160  printInfoMessage("LdmrsFieldApp::thread_removeAllFields: Removing field " + toString(i) + ".", beVerboseHere);
161  result = ldmrs->writeField(i, field);
162  if (result == false)
163  {
164  // Failure
165  printError("LdmrsFieldApp::thread_removeAllFields: Failed to clear field " + toString(i) + "!");
166  }
167  }
168 
169  printInfoMessage("LdmrsFieldApp::thread_removeAllFields: All done, leaving.", beVerboseHere);
170 }
171 
172 //
173 // Demo function that creates an eval case.
174 //
176 {
177  bool beVerboseHere = m_beVerbose;
178  beVerboseHere = true;
179 
180  printInfoMessage("LdmrsFieldApp::thread_createEvalCase(): Called.", beVerboseHere);
181 
182  bool result;
183 
184  // The single eval case
185  EvalCase* c = new EvalCase;
186  c->setVersionNumber(1);
187  c->setCaseNumber(1); // Case number must be >0 for a valid case
189  c->setResultNegation(false);
190  c->setResponseTime(500); // ms
191  c->setResponseTimeExtended(500); // ms
192  c->setOutputNumber(0); // 0 = none
193  c->setLogicalInputState_from_UINT8(0x00); // 0 = no inputs
194  c->setDistDependent(false);
195  c->setMaxRadialCorridor(2.0);
197  c->setBlankingSize(0.2);
198  c->setMinFieldExp(0);
199  c->setFieldNumber(1);
201  c->setCaseName("DemoCase-1");
202  c->setComment("");
203 
204  // The cases carrier structure
205  EvalCases cases;
206  cases.add(c);
207 
208  result = ldmrs->writeEvalCases(cases);
209  if (result == false)
210  {
211  // Failure
212  printError("LdmrsFieldApp::thread_createEvalCase: Failed to write the EvalCase structure!");
213  }
214 
215  printInfoMessage("LdmrsFieldApp::thread_createEvalCase: All done, leaving.", beVerboseHere);
216 }
217 
218 //
219 // Demo function that creates a rectangular field.
220 //
222 {
223  bool beVerboseHere = m_beVerbose;
224  beVerboseHere = true;
225 
226  printInfoMessage("LdmrsFieldApp::thread_createRectangularField(): Called.", beVerboseHere);
227 
228  // Create an empty field which is invalid
229  FieldParameter field;
230  field.setDistScaleFactor(10); // With a value of 10, coordinate unit is [cm]
231  field.setDistScaleOffset(0); // No offset
232  field.setAngleScaleFactor(8); // Angle unit is [1/4 deg]
233  field.setAngleScaleOffset(-1920); // -1920 = -60 deg (=60 deg due to other coordinate system)
234  field.setComment("");
235  field.setFieldName("Rect_1");
236  field.setEnableLayerFilter(false);
237  field.setVersionNumber(1); // Version number is 1
239  field.setFieldNumber(1); // A field number 0 marks the field as invalid
240 
241  // Create a rectangular field sub-structure
242  FieldRectangle rectField;
243 // FieldSegmentedPoint segPt;
244 
245  rectField.setRefPointDist(2.06); // Dist to ref point is 2.06 m
246  rectField.setRefPointAngle(13.8 * deg2rad); // Angle of 13.8 deg
247  rectField.setRotAngle(0.0 * deg2rad); // 0 = No rotation
248  rectField.setLength(5.0); // Length 5 m (x direction)
249  rectField.setWidth(2.0); // Width 2 m (y direction)
250 
251  // Add the rectangular field to the field
252  field.setField(&rectField); // (FieldDescription*)
253 
254  // Write the field as field 1 to the sensor
255  bool result = false;
256  result = ldmrs->writeField(0, field);
257  if (result == false)
258  {
259  // Failure
260  printError("LdmrsFieldApp::thread_createRectangularField(): Failed to write the field!");
261  }
262 
263  printInfoMessage("LdmrsFieldApp::thread_createRectangularField(): All done, leaving.", beVerboseHere);
264 }
265 
266 //
267 // Thread that does the actual changing of parameters.
268 //
269 void LdmrsFieldApp::changeThreadFunction(bool& endThread, UINT16& waitTimeMs)
270 {
271  bool beVerboseHere = m_beVerbose;
272  beVerboseHere = true;
273 
274  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): started", beVerboseHere);
275 
276  devices::LDMRS* ldmrs;
277 
278  // Wait until the LD-MRS is present
279  for (UINT16 i = 0; i<10; i++)
280  {
282  if (ldmrs == NULL)
283  {
284  // Sleep for a second
285  usleep(1000000);
286  }
287  }
288 
289  if (ldmrs == NULL)
290  {
291  // Scanner device not found
292  printError("LdmrsFieldApp::changeThreadFunction(): Failed to read device pointer, aborting!");
293  endThread = true;
294  waitTimeMs = 0;
295  return;
296  }
297 
298  // Now wait until the connection is established
299  for (UINT16 i = 0; i<10; i++)
300  {
301  if (ldmrs->isRunning() == false)
302  {
303  // Sleep for a second
304  usleep(1000000);
305  }
306  }
307  // Sleep some more, just to be sure
308  usleep(3 * 1000000);
309 
310  UINT32 sleepTimeS;
311 
312 
313  //
314  // Remove all existing eval cases
315  //
316  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now removing all eval cases.", beVerboseHere);
318 
319 
320  //
321  // Remove all exisiting fields by overwriting them with invalid fields.
322  //
323  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now removing all fields.", beVerboseHere);
324  thread_removeAllFields(ldmrs);
325 
326  // sleep some time
327  sleepTimeS = 3;
328  usleep(sleepTimeS * 1000 * 1000);
329 
330 
331  //
332  // Write a single rectangular field to the sensor.
333  //
334  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now writing a rectangular field.", beVerboseHere);
336 
337  // sleep some time
338  sleepTimeS = 1;
339  usleep(sleepTimeS * 1000 * 1000);
340 
341 
342  //
343  // Write a single eval case to the sensor.
344  //
345  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now writing an eval case for the rectangular field.", beVerboseHere);
346  thread_createEvalCase(ldmrs);
347 
348 
349  //
350  // Save the current field configuration permanently.
351  //
352 // printInfoMessage("LdmrsFieldApp::changeThreadFunction(): Now flashing the current field configuration.", beVerboseHere);
353 // ldmrs->flashSopasConfig();
354 
355  // sleep some time
356  sleepTimeS = 5;
357  usleep(sleepTimeS * 1000 * 1000);
358 
359 
360  endThread = true;
361  waitTimeMs = 0;
362  printInfoMessage("LdmrsFieldApp::changeThreadFunction(): All done, leaving.", m_beVerbose);
363 }
364 
365 
366 } // namespace application
void printError(std::string message)
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
void setMinFieldExp(double minFieldExp)
Definition: EvalCase.cpp:195
void setResponseTime(UINT32 responseTime)
Definition: EvalCase.cpp:205
void setDistDependent(bool distDependent)
Definition: EvalCase.cpp:160
void setStrategy(EvaluationStrategy strategy)
Definition: EvalCase.cpp:225
bool writeEvalCases(const EvalCases &evalCases)
Definition: LD_MRS.cpp:251
bool writeField(UINT16 fieldNum, const FieldParameter &para)
Definition: LD_MRS.cpp:222
uint16_t UINT16
LdmrsFieldApp(Manager *manager)
void add(EvalCase_ptr evalCase)
Definition: EvalCases.cpp:27
void setFieldNumber(UINT16 fieldNumber)
Definition: EvalCase.cpp:170
Definition: Time.hpp:44
devices::BasicDevice * getFirstDeviceByType(Sourcetype type)
Definition: manager.cpp:412
void setDistScaleFactor(double distScaleFactor)
#define printInfoMessage(a, b)
void setAngleScaleFactor(UINT32 angleScaleFactor)
uint32_t UINT32
void setAngleScaleOffset(INT32 angleScaleOffset)
void setField(FieldDescription *field)
virtual bool isRunning()
Definition: LD_MRS.cpp:557
const Time & getStartTimestamp() const
Definition: ScannerInfo.hpp:73
void thread_createEvalCase(devices::LDMRS *ldmrs)
void setCaseName(const std::string &caseName)
Definition: EvalCase.cpp:133
#define deg2rad
void setOutputNumber(UINT8 outputNumber)
Definition: EvalCase.cpp:200
void setRefPointDist(double refPointDist)
Definition: Fields.cpp:143
void thread_removeAllFields(devices::LDMRS *ldmrs)
void setData(BasicData &data)
SickThread< LdmrsFieldApp,&LdmrsFieldApp::changeThreadFunction > m_changeThread
void setRotAngle(double rotAngle)
Definition: Fields.cpp:152
void changeThreadFunction(bool &endThread, UINT16 &waitTimeMs)
void setDistScaleOffset(double distScaleOffset)
void setWidth(double width)
Definition: Fields.cpp:163
void setVersionNumber(UINT16 versionNumber)
Definition: EvalCase.cpp:233
void setBlankingSize(double blankingSize)
Definition: EvalCase.cpp:122
void setFieldTypeIntern(UINT8 fieldTypeIntern)
void setLogicalInputState_from_UINT8(UINT8 value)
Definition: EvalCase.cpp:260
void setCaseNumber(UINT8 caseNumber)
Definition: EvalCase.cpp:142
void setResultNegation(bool resultNegation)
Definition: EvalCase.cpp:215
const ScannerInfo * getScannerInfoByDeviceId(UINT8 id) const
Definition: Scan.cpp:360
void setResponseTimeExtended(UINT32 responseTimeExtended)
Definition: EvalCase.cpp:210
void setMaxRadialCorridor(double maxRadialCorridor)
Definition: EvalCase.cpp:190
void setFilterType(FilterType filterType)
Definition: EvalCase.cpp:175
void setManipulationPrevention(ManipulationPrevention manPrev)
Definition: EvalCase.cpp:251
void setComment(const std::string &comment)
void setComment(const std::string &comment)
Definition: EvalCase.cpp:151
void thread_createRectangularField(devices::LDMRS *ldmrs)
void setRefPointAngle(double refPointAngle)
Definition: Fields.cpp:138
void setVersionNumber(UINT8 m_versionNumber)
void thread_removeAllEvalCases(devices::LDMRS *ldmrs)
void setFieldNumber(UINT16 m_fieldNumber)
void setLength(double length)
Definition: Fields.cpp:126
void setEnableLayerFilter(bool enableLayerFilter)
void setFieldName(const std::string &fieldName)
std::string toString() const
Definition: Time.cpp:40


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33