LD_MRS.cpp
Go to the documentation of this file.
1 //
2 // LD_MRS.cpp
3 //
4 // Device class of a simple LD-MRS laserscanner interface.
5 //
6 //
7 // To configure the device, add the parameters in the constructor below.
8 //
9 // VERSION 1.1.0
10 //
11 // Version history:
12 // 1.0.0 , willhvo
13 // - Initial version
14 //
15 // 1.1.0 , willhvo
16 // - Added reading from file
17 //
18 
19 
20 //
21 #include "LD_MRS.hpp"
22 #include "../tools/errorhandler.hpp"
23 
24 namespace devices
25 {
26 
27 using namespace datatypes;
28 
30  : m_manager(manager)
31  , m_sopas(NULL)
32  , m_lux(NULL)
33 {
35  m_beVerbose = false;
36 
37  printInfoMessage("LDMRS::LDMRS: Starting constructor.", m_beVerbose);
38 
39  m_lux = NULL;
40 
41  m_weWantScanData = true; // Enable or disable scan data reception via the MRS (=non-SOPAS) interface.
42  m_weWantObjectData = false; // Enable or disable object data reception via the MRS (=non-SOPAS) interface.
43  m_weWantFieldData = false; // Enable or disable protection field and eval case data reception via the SOPAS interface.
44  m_weWantScanDataFromSopas = false; // Enable or disable scan data reception via the SOPAS interface.
45 
46 // 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.
47  m_inputFileName = "";
48  m_ipAddress = "192.168.0.1"; // , "Network IP address where the device is located.", "0.0.0.0");
49  m_luxPortNumber = 12002; // Network port on which the legacy (non-sopas) device is contacted (for scans and objects), typically 12002
50  m_SopasPortNumber = 2111; // Network port on which the device is contacted, typically 2111 or 2112.
51 
52  // parameters for LuxBase/Scans
53  m_scanFrequency = 12.5; // Scan frequency (12.5, 25 or 50 Hz)
54  m_scanStartAngle = 45.0 * deg2rad; // Start angle of the scan.
55  m_scanEndAngle = -45.0 * deg2rad; // End angle of the scan.
56  m_readOnlyMode = false; // Forbids parameter writes to the scanner if true.
57 
58 // m_scanAngleRes = 0.25;
59 // m_skipNumScans = 0; // Use this value if the sensor scan frequency is too high and if scans should be skipped.
60 
61  m_offsetX = 0.0; // x-coordinate of sensor mounting position in the vehicle coordinate system.
62  m_offsetY = 0.0; // y-coordinate of sensor mounting position in the vehicle coordinate system
63  m_offsetZ = 0.0; // z-coordinate of sensor mounting position in the vehicle coordinate system
64  m_yawAngle = 0.0; // Yaw angle of sensor mounting position in the vehicle coordinate system ("left and right")
65  m_pitchAngle = 0.0; // Pitch angle of sensor mounting position in the vehicle coordinate system ("up and down")
66  m_rollAngle = 0.0; // Roll angle of sensor mounting position in the vehicle coordinate system ("to the sides")
67 
68  printInfoMessage("LDMRS::LDMRS: Constructor done.", m_beVerbose);
69 }
70 
71 
72 
74 {
75  printInfoMessage("LDMRS::~LDMRS: Starting destructor.", m_beVerbose);
76 
77  if (isRunning())
78  {
79  stop();
80  }
81 
82  printInfoMessage("LDMRS::~LDMRS: Destructor done.", m_beVerbose);
83 }
84 
85 
86 //
87 // Initialising.
88 //
89 // It would be nicer if this function would be called with all the device parameters,
90 // instead of setting them as module variables...
91 //
93 {
94  printInfoMessage("LDMRS::init: Called.", m_beVerbose);
95 
96  std::string longName = "LD-MRS";
97  bool result = true;
98 
99 // m_beVerbose = false;
100  m_isRunning = false;
101 
102  // Altes Objekt loeschen
103  if (m_lux != NULL)
104  {
105  m_lux->stop();
106  delete m_lux;
107  m_lux = NULL;
108  }
109 
110  // initializing LUX part
111  printInfoMessage("LDMRS::init: Creating new LuxBase object.", m_beVerbose);
112  m_lux = new LuxBase(m_manager,
113  getSourceId(),
114  longName,
115  m_ipAddress,
120  m_offsetX,
121  m_offsetY,
122  m_offsetZ,
123  m_yawAngle,
124  m_pitchAngle,
125  m_rollAngle,
126  m_beVerbose,
128  );
129 
130  printInfoMessage("LDMRS::init: Initializing the LuxBase object.", m_beVerbose);
131  if (m_inputFileName == "")
132  {
133  // TCP
134  result = m_lux->initTcp(LDMRS::disconnectFunctionS, (void*)this);
135  }
136  else
137  {
138  // File
139  result = m_lux->initFile(LDMRS::disconnectFunctionS, (void*)this);
140  }
141 
142  if (result == true)
143  {
144  // Success
145  printInfoMessage("LDMRS::init: LuxBase was successfully initialized.", m_beVerbose);
146  }
147  else
148  {
149  // Error
150  printError("LDMRS::init: Failed to initialize LuxBase - Device init failed, aborting.");
151  return false;
152  }
153 
154  // Set the current time
155  Time t = Time::now();
156  if (m_inputFileName == "")
157  {
158  // TCP
160  if (result == true)
161  {
162  // Success
163  printInfoMessage("LDMRS::init: NTP timestamp was successfully set.", m_beVerbose);
164  }
165  else
166  {
167  // Error
168  printError("LDMRS::init: Failed to set NTP timestamp - Device init failed, aborting.");
169  return false;
170  }
171  }
172 
173 
174  return true;
175 }
176 
177 
178 //
179 // Static-Einstiegspunkt fuer die DisconnectFunction.
180 //
182 {
183  printInfoMessage("LDMRS::disconnectFunctionS: Called.", true);
184 
185  if (obj != NULL)
186  {
187  ((LDMRS*)obj)->disconnectFunction();
188  }
189 
190 // SystemStateWatchdog::setNotConnected();
191 
192  printInfoMessage("LDMRS::disconnectFunctionS: Done.", true);
193 }
194 
195 //
196 //
197 //
199 {
200  printInfoMessage("LDMRS::disconnectFunction: LDMRS " + getDeviceName() + " was disconnected.", m_beVerbose);
201 
202 // SystemStateWatchdog::setNotConnected();
203 }
204 
205 
206 //
207 // Schliesse alle Schnittstellen.
208 //
210 {
211  printInfoMessage("LDMRS::shutdown: called. Shutting down the LDMRS" + getDeviceName() + ".", m_beVerbose);
212 
213  if (m_lux->isRunning())
214  {
215  stop();
216  }
217 }
218 
219 //
220 // Write a field to the sensor
221 //
222 bool LDMRS::writeField(UINT16 fieldNum, const FieldParameter& para)
223 {
224  bool result;
225  bool beVerboseHere = m_beVerbose;
226  beVerboseHere = true;
227 
228  printInfoMessage("LDMRS::writeField: Called. Logging in now.", beVerboseHere);
229  result = m_sopas->action_login();
230 
231  if (result == true)
232  {
233  printInfoMessage("LDMRS::writeField: Login was successful, writing field now.", beVerboseHere);
234  result = m_sopas->action_writeField(fieldNum, para);
235  }
236 
237  if (result == true)
238  {
239  printInfoMessage("LDMRS::writeField: Field was written, logging out now.", beVerboseHere);
240  result = m_sopas->action_logout();
241  }
242 
243  printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
244  return result;
245 }
246 
247 //
248 // Write an EvalCase configuration to the sensor.
249 // This is a structure with all (up to 16) eval cases.
250 //
251 bool LDMRS::writeEvalCases(const EvalCases& evalCases)
252 {
253  bool result;
254  bool beVerboseHere = m_beVerbose;
255  beVerboseHere = true;
256 
257  printInfoMessage("LDMRS::writeEvalCases: Called. Logging in now.", beVerboseHere);
258  result = m_sopas->action_login();
259 
260  if (result == true)
261  {
262  printInfoMessage("LDMRS::writeEvalCases: Login was successful, writing eval cases now.", beVerboseHere);
263  result = m_sopas->action_writeEvalCases(evalCases);
264  }
265 
266  if (result == true)
267  {
268  printInfoMessage("LDMRS::writeEvalCases: Eval cases were written, logging out now.", beVerboseHere);
269  result = m_sopas->action_logout();
270  }
271 
272  printInfoMessage("LDMRS::writeEvalCases: All done, leaving.", beVerboseHere);
273  return result;
274 }
275 
276 //
277 // Stores the SOPAS config data (fields and eval cases) permanently.
278 //
280 {
281  bool result;
282  bool beVerboseHere = m_beVerbose;
283  beVerboseHere = true;
284 
285  printInfoMessage("LDMRS::flashSopasConfig: Called.", beVerboseHere);
286 
288  if (result == true)
289  {
290  printInfoMessage("LDMRS::flashSopasConfig: Configuration was saved.", beVerboseHere);
291  }
292  else
293  {
294  // Failure
295  printError("LDMRS::flashSopasConfig: Failed to save configuration!");
296  }
297 
298  printInfoMessage("LDMRS::writeField: All done, leaving.", beVerboseHere);
299  return result;
300 }
301 
302 
303 //
304 // Sets the MRS-internal clock to the given time.
305 //
306 bool LDMRS::setNtpTime(UINT32 seconds, UINT32 fractionalSec)
307 {
308  bool result;
309  bool beVerboseHere = m_beVerbose;
310  beVerboseHere = true;
311 
312  printInfoMessage("LDMRS::setNtpTime: Called.", beVerboseHere);
313 
314  if (m_lux == NULL)
315  {
316  printError("LDMRS::setNtpTime: No LUX-Base object available, aborting!");
317  return false;
318  }
319 
320  result = m_lux->cmd_setNtpTimestamp(seconds, fractionalSec);
321  if (result == true)
322  {
323  printInfoMessage("LDMRS::setNtpTime: Timestamp was set.", beVerboseHere);
324  }
325  else
326  {
327  // Failure
328  printError("LDMRS::setNtpTime: Failed to set timestamp!");
329  }
330 
331  printInfoMessage("LDMRS::setNtpTime: All done, leaving.", beVerboseHere);
332  return result;
333 }
334 
335 bool LDMRS::setScanAngles(double startAngle, double endAngle)
336 {
337  bool result;
338  bool beVerboseHere = m_beVerbose;
339 
340  printInfoMessage("LDMRS::setScanAngles: Called.", beVerboseHere);
341 
342  if (m_lux == NULL)
343  {
344  printError("LDMRS::setScanAngles: No LUX-Base object available, aborting!");
345  return false;
346  }
347 
348  result = m_lux->cmd_setScanAngles(startAngle, endAngle);
349  if (result == true)
350  {
351  printInfoMessage("LDMRS::setScanAngles: scan angles were set.", beVerboseHere);
352  }
353  else
354  {
355  // Failure
356  printError("LDMRS::setScanAngles: Failed to set scan angles!");
357  }
358 
359  printInfoMessage("LDMRS::setScanAngles: All done, leaving.", beVerboseHere);
360  return result;
361 }
362 
363 bool LDMRS::setSyncAngleOffset(double syncAngle)
364 {
365  bool result;
366  bool beVerboseHere = m_beVerbose;
367 
368  printInfoMessage("LDMRS::setSyncAngleOffset: Called.", beVerboseHere);
369 
370  if (m_lux == NULL)
371  {
372  printError("LDMRS::setSyncAngleOffset: No LUX-Base object available, aborting!");
373  return false;
374  }
375 
376  result = m_lux->cmd_setSyncAngleOffset(syncAngle);
377  if (result == true)
378  {
379  printInfoMessage("LDMRS::setSyncAngleOffset: sync angle offset was set.", beVerboseHere);
380  }
381  else
382  {
383  // Failure
384  printError("LDMRS::setSyncAngleOffset: Failed to set sync angle offset!");
385  }
386 
387  printInfoMessage("LDMRS::setSyncAngleOffset: All done, leaving.", beVerboseHere);
388  return result;
389 }
390 
391 bool LDMRS::setScanFrequency(double scanFreq)
392 {
393  bool result;
394  bool beVerboseHere = m_beVerbose;
395 
396  printInfoMessage("LDMRS::setScanFrequency: Called.", beVerboseHere);
397 
398  if (m_lux == NULL)
399  {
400  printError("LDMRS::setScanFrequency: No LUX-Base object available, aborting!");
401  return false;
402  }
403 
404  result = m_lux->cmd_setScanFrequency(scanFreq);
405  if (result == true)
406  {
407  printInfoMessage("LDMRS::setScanFrequency: scan frequency was set.", beVerboseHere);
408  }
409  else
410  {
411  // Failure
412  printError("LDMRS::setScanFrequency: Failed to set scan frequency!");
413  }
414 
415  printInfoMessage("LDMRS::setScanFrequency: All done, leaving.", beVerboseHere);
416  return result;
417 }
418 
419 std::string LDMRS::getIpAddress()
420 {
421  return m_ipAddress;
422 }
423 
424 void LDMRS::setIpAddress(std::string ipAdress)
425 {
426  m_ipAddress = ipAdress;
427 }
428 
429 void LDMRS::setWeWantObjectData(bool weWantObjectData) {
430  m_weWantObjectData = weWantObjectData;
431 }
432 
434 {
435  if (m_lux == NULL)
436  {
437  return "(none)";
438  } else {
439  return m_lux->getSerialNumber();
440  }
441 }
442 
444 {
445  if (m_lux == NULL)
446  {
447  return "(none)";
448  } else {
449  return m_lux->getFirmwareVersion();
450  }
451 }
452 
453 
454 //
455 // Starte das Einlesen von Daten (Scans, Objekte, Schutzfelder, ...)
456 //
458 {
459  if (m_lux == NULL)
460  {
461  printError("LDMRS::run: called, but pointer to internal data handler is NULL. Was init() called? Aborting!");
462  return false;
463  }
464 
465  //
466  // LUX-Part
467  //
468  printInfoMessage("LDMRS::run: Called. Run the LDMRS" + getDeviceName() + ".", m_beVerbose);
469 
470  // This will cause that we get scan and/or obect data via lux protocol
471  bool luxSuccess = m_lux->run(m_weWantScanData, m_weWantObjectData);
472  if (luxSuccess == true)
473  {
474  // Success
475  printInfoMessage("LDMRS::run: Device " + getDeviceName() + " is running.", m_beVerbose);
477  }
478  else
479  {
480  // Fail
481  printError("LDMRS::run: Failed to run device " + getDeviceName() + ", aborting!");
482  return false;
483  }
484 
485  //
486  // SOPAS part
487  //
488  if ((m_inputFileName == "") &&
489  ((m_weWantFieldData == true) ||
490  (m_weWantScanDataFromSopas == true)))
491  {
492  // TCP only
493  bool sopasSuccess = true;
494  printInfoMessage("LDMRS::run: Now starting SOPAS part (eval cases) of device " + getDeviceName() + ".", m_beVerbose);
495 
496  // Initialize Sopas part
498  getSourceId(),
499  m_ipAddress,
504  sopasSuccess = m_sopas->init(LDMRS::disconnectFunctionS, (void*)this);
505 
506  if (sopasSuccess == false)
507  {
508  printError("LDMRS::run: Failed to initialize SOPAS part of device " + getDeviceName() + ", aborting!");
509  stop();
510  return false;
511  }
512 
513  // Success, now run the device
514  printInfoMessage("LDMRS::run: SOPAS part is initialized, now running it.", m_beVerbose);
515  sopasSuccess = m_sopas->run();
516  if (sopasSuccess == false)
517  {
518  printError("LDMRS::run: Failed to run SOPAS part of device " + getDeviceName() + ", aborting!");
519  stop();
520  return false;
521  }
522 
523  printInfoMessage("LDMRS::run: SOPAS part is running, device " + getDeviceName() + " is running, all done.", m_beVerbose);
524  }
525 
526  return true;
527 }
528 
529 
530 //
531 // Stop data reception, but do not shut down the connection.
532 //
534 {
535  printInfoMessage("LDMRS::stop: Called. Stopping the LDMRS" + getDeviceName() + ".", m_beVerbose);
536 
537  bool luxSuccess = true;
538  bool sopasSuccess = true;
539 
540  if (m_sopas != NULL)
541  {
543  {
544  sopasSuccess = m_sopas->action_unSubscribeEvalCaseResults();
545  }
546  }
547 
548  if (m_lux != NULL)
549  {
550  luxSuccess = m_lux->stop();
551  }
552 
553  return (luxSuccess && sopasSuccess);
554 }
555 
556 
558 {
559  if (m_lux == NULL) // no m_lux object yet? Is not running.
560  {
561  return false;
562  }
563  bool luxSuccess = m_lux->isRunning();
564  //bool sopasSuccess = m_sopas.isRunning();
565  return (luxSuccess); // && sopasSuccess
566 }
567 
568 //
569 // Statischer Einstiegspunkt
570 //
571 void LDMRS::onScanReceivedS(void* obj)
572 {
573  if (obj != NULL)
574  {
575  ((LDMRS*)obj)->onScanReceived();
576  }
577 }
578 
579 // after / before every scan we have to post the current sensor state as SensorInfo Structure
581 {
582 // printInfoMessage("LDMRS::onScanReceived: Called.", m_beVerbose);
583 
584 // SensorStateInfo sensorInfo = m_sopas.getSensorStateInfo();
585 // getSerializer().notifyMessageHandlers(sensorInfo, getDeviceID());
586 }
587 
589 {
590  return m_lux->cmd_getParameter(id, value);
591 }
592 
594 {
595  return m_lux->cmd_setParameter(id, value);
596 }
597 
598 
599 } // namespace devices
600 
double seconds()
Definition: Time.cpp:102
static const UINT64 secondsFrom1900to1970
Definition: Time.hpp:72
void printError(std::string message)
virtual bool run()
Definition: LD_MRS.cpp:457
void disconnectFunction()
Definition: LD_MRS.cpp:198
bool cmd_getParameter(MrsParameterId parameter, UINT32 *value)
Definition: LuxBase.cpp:851
std::string getFirmwareVersion()
Definition: LuxBase.hpp:292
Manager * m_manager
Definition: LD_MRS.hpp:28
bool writeEvalCases(const EvalCases &evalCases)
Definition: LD_MRS.cpp:251
virtual bool isRunning()
Definition: LuxBase.cpp:304
bool writeField(UINT16 fieldNum, const FieldParameter &para)
Definition: LD_MRS.cpp:222
uint16_t UINT16
bool cmd_setSyncAngleOffset(double syncAngle)
Definition: LuxBase.cpp:538
double m_scanFrequency
Definition: LD_MRS.hpp:48
bool m_weWantScanDataFromSopas
Definition: LD_MRS.hpp:39
bool setSyncAngleOffset(double syncAngle)
Definition: LD_MRS.cpp:363
virtual bool init()
Definition: LD_MRS.cpp:92
Definition: Time.hpp:44
bool m_weWantScanData
Definition: LD_MRS.hpp:36
#define printInfoMessage(a, b)
LuxBase * m_lux
Definition: LD_MRS.hpp:30
bool setScanAngles(double startAngle, double endAngle)
Definition: LD_MRS.cpp:335
uint32_t UINT32
double m_offsetY
Definition: LD_MRS.hpp:52
void setOnScanReceiveCallbackFunction(OnScanReceiveCallbackFunction function, void *obj)
Definition: LuxBase.cpp:108
static void disconnectFunctionS(void *obj)
Definition: LD_MRS.cpp:181
std::string getSerialNumber()
Definition: LD_MRS.cpp:433
virtual bool isRunning()
Definition: LD_MRS.cpp:557
void setIpAddress(std::string ipAddress)
Definition: LD_MRS.cpp:424
LdmrsSopasLayer * m_sopas
Definition: LD_MRS.hpp:29
virtual bool initTcp(Tcp::DisconnectFunction function, void *obj)
Definition: LuxBase.cpp:118
#define deg2rad
MrsParameterId
Definition: LuxBase.hpp:51
bool m_weWantObjectData
Definition: LD_MRS.hpp:38
virtual void shutdown()
Definition: LD_MRS.cpp:209
double m_scanStartAngle
Definition: LD_MRS.hpp:49
std::string m_inputFileName
Definition: LD_MRS.hpp:45
virtual bool stop()
Definition: LuxBase.cpp:2772
UINT16 m_luxPortNumber
Definition: LD_MRS.hpp:43
bool action_writeField(UINT16 fieldNum, const FieldParameter &para)
bool cmd_setParameter(MrsParameterId parameter, UINT32 value)
Definition: LuxBase.cpp:810
bool action_writeEvalCases(const EvalCases &evalCases)
bool getParameter(MrsParameterId id, UINT32 *value)
Definition: LD_MRS.cpp:588
bool setParameter(MrsParameterId id, UINT32 value)
Definition: LD_MRS.cpp:593
double m_offsetZ
Definition: LD_MRS.hpp:53
bool flashSopasConfig()
Definition: LD_MRS.cpp:279
std::string m_ipAddress
Definition: LD_MRS.hpp:42
double m_offsetX
Definition: LD_MRS.hpp:51
virtual bool stop()
Definition: LD_MRS.cpp:533
virtual bool initFile(File::DisconnectFunction function, void *obj)
Definition: LuxBase.cpp:236
void setWeWantObjectData(bool weWantObjectData)
Definition: LD_MRS.cpp:429
static void onScanReceivedS(void *obj)
Definition: LD_MRS.cpp:571
virtual bool run(bool weWantScanData=true, bool weWantObjectData=true)
Definition: LuxBase.cpp:2611
bool cmd_setScanFrequency(double scanFreq)
Definition: LuxBase.cpp:417
bool cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec)
Definition: LuxBase.cpp:732
bool setScanFrequency(double scanFreq)
Definition: LD_MRS.cpp:391
void setDevicetype(UINT16 devicetype)
Definition: BasicDevice.cpp:42
double m_yawAngle
Definition: LD_MRS.hpp:54
double m_rollAngle
Definition: LD_MRS.hpp:56
UINT16 m_SopasPortNumber
Definition: LD_MRS.hpp:44
bool m_weWantFieldData
Definition: LD_MRS.hpp:37
bool m_beVerbose
Definition: LD_MRS.hpp:32
std::string getFirmwareVersion()
Definition: LD_MRS.cpp:443
bool m_readOnlyMode
Definition: LD_MRS.hpp:41
bool setNtpTime(UINT32 seconds, UINT32 fractionalSec)
Definition: LD_MRS.cpp:306
bool m_isRunning
Definition: LD_MRS.hpp:33
std::string getDeviceName()
Definition: BasicDevice.cpp:74
double m_pitchAngle
Definition: LD_MRS.hpp:55
std::string getSerialNumber()
Definition: LuxBase.cpp:1170
std::string getIpAddress()
Definition: LD_MRS.cpp:419
bool cmd_setScanAngles(double startAngle, double endAngle)
Definition: LuxBase.cpp:462
static Time now()
Definition: Time.cpp:168
bool isFieldDataSubscribed() const
bool init(Tcp::DisconnectFunction function, void *obj)
LDMRS(Manager *manager)
Definition: LD_MRS.cpp:29
double m_scanEndAngle
Definition: LD_MRS.hpp:50
void onScanReceived()
Definition: LD_MRS.cpp:580


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