Main MRPT website > C++ reference for MRPT 1.5.7
CSickLaserSerial.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 // This file contains portions of code from sicklms200.cc from the Player/Stage project.
11 
12 #include "hwdrivers-precomp.h" // Precompiled headers
13 
14 #include <mrpt/utils/crc.h>
15 #include <mrpt/utils/CTicTac.h>
16 #include <mrpt/system/os.h>
17 
19 
20 
22 
23 #define RET_ERROR(msg) { cout << "[" << __CURRENT_FUNCTION_NAME__ <<"] " << msg << endl; return false; }
24 
25 using namespace std;
26 using namespace mrpt;
27 using namespace mrpt::utils;
28 using namespace mrpt::obs;
29 using namespace mrpt::poses;
30 using namespace mrpt::hwdrivers;
31 
32 
33 int CSickLaserSerial::CRC16_GEN_POL = 0x8005;
34 
35 /*-------------------------------------------------------------
36  CSickLaserSerial
37 -------------------------------------------------------------*/
38 CSickLaserSerial::CSickLaserSerial() :
39  m_mm_mode( false ),
40  m_scans_FOV(180),
41  m_scans_res(50),
42  m_com_port(),
43  m_mySerialPort( NULL ),
44  m_com_baudRate(38400),
45  m_nTries_connect(1),
46  m_nTries_current(0),
47  m_skip_laser_config(false)
48 {
49  m_sensorLabel = "SICKLMS";
51 }
52 
53 /*-------------------------------------------------------------
54  ~CSickLaserSerial
55 -------------------------------------------------------------*/
57 {
58  if (m_stream)
59  {
60  try
61  {
63  {
65  }
66  }
67  catch(...) {}
68  }
69 
70  if (m_mySerialPort)
71  {
72  delete m_mySerialPort;
73  m_mySerialPort = NULL;
74  }
75 }
76 
77 /*-------------------------------------------------------------
78  doProcessSimple
79 -------------------------------------------------------------*/
81  bool &outThereIsObservation,
82  mrpt::obs::CObservation2DRangeScan &outObservation,
83  bool &hardwareError )
84 {
85  outThereIsObservation = false;
86  hardwareError = false;
87 
88  if ( !tryToOpenComms() )
89  {
90  hardwareError = true;
91  return;
92  }
93 
94  vector<float> ranges;
95  unsigned char LMS_stat;
96  bool is_mm_mode;
97 
99 
100  // Wait for a scan:
101  if (!waitContinuousSampleFrame( ranges,LMS_stat, is_mm_mode ))
102  return;
103 
104  // Yes, we have a new scan:
105 
106  // -----------------------------------------------
107  // Extract the observation:
108  // -----------------------------------------------
109  outObservation.timestamp = mrpt::system::now();
110 
111  outObservation.sensorLabel = m_sensorLabel; // Set label
112 
113  // Extract the timestamp of the sensor:
114 
115  // And the scan ranges:
116  outObservation.rightToLeft = true;
117  outObservation.aperture = M_PIf;
118  outObservation.maxRange = is_mm_mode ? 32.7 : 81.0;
119  outObservation.stdError = 0.003f;
120  outObservation.sensorPose = mrpt::poses::CPose3D(m_sensorPose);
121 
122  outObservation.resizeScan(ranges.size());
123 
124  for (size_t i=0;i<ranges.size();i++) {
125  outObservation.setScanRange( i, ranges[i] );
126  outObservation.setScanRangeValidity(i, (outObservation.scan[i] <= outObservation.maxRange) );
127  }
128 
129  // Do filter:
132  // Do show preview:
134 
135  outThereIsObservation = true;
136 }
137 
138 /*-------------------------------------------------------------
139  loadConfig_sensorSpecific
140 -------------------------------------------------------------*/
142  const mrpt::utils::CConfigFileBase &configSource,
143  const std::string &iniSection )
144 {
146  configSource.read_float(iniSection,"pose_x",0),
147  configSource.read_float(iniSection,"pose_y",0),
148  configSource.read_float(iniSection,"pose_z",0),
149  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
150  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
151  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
152  );
153 
154  m_mm_mode = configSource.read_bool(iniSection,"mm_mode",m_mm_mode);
155 
156 #ifdef MRPT_OS_WINDOWS
157  m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true );
158 #else
159  m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true );
160 #endif
161 
162  m_com_baudRate = configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate );
163  m_nTries_connect = configSource.read_int(iniSection, "nTries_connect", m_nTries_connect );
164 
165  m_scans_FOV = configSource.read_int(iniSection, "FOV", m_scans_FOV );
166  m_scans_res = configSource.read_int(iniSection, "resolution", m_scans_res );
167 
168  m_skip_laser_config = configSource.read_bool(iniSection, "skip_laser_config", m_skip_laser_config );
169 
170  // Parent options:
171  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
172 }
173 
174 /*-------------------------------------------------------------
175  turnOn
176 -------------------------------------------------------------*/
178 {
179  return true;
180 }
181 
182 /*-------------------------------------------------------------
183  turnOff
184 -------------------------------------------------------------*/
186 {
187  return true;
188 }
189 
190 /*-------------------------------------------------------------
191  Tries to open the com port and setup
192  all the LMS protocol. Returns true if OK or already open.
193 -------------------------------------------------------------*/
195 {
196  if (err_msg) *err_msg="";
197  try
198  {
199  if (!m_stream)
200  {
201  ASSERT_(m_mySerialPort==NULL);
202 
203  // There is no COMMS port open yet...
204  if (!m_com_port.empty())
205  {
206  // Create the port myself:
207  m_mySerialPort = new CSerialPort();
209  }
210  else
211  throw std::logic_error("ERROR: No serial port attached with bindIO, neither it set with 'setSerialPort'");
212  }
213 
214  // We assure now we have a stream... try to open it, if it's not done yet.
215  bool just_open = false;
216  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
217  if (COM!=NULL)
218  {
219  if (!COM->isOpen())
220  {
221  // Try to open it now:
223  COM->open(); // will raise an exception on error.
224 
225  // Set basic params:
226  COM->setConfig(9600);
227  COM->setTimeouts(100,0,10,0,50);
228 
229  just_open = true;
230  }
231  }
232 
233  // It seems the port was open and working so we are done here.
234  if (!just_open)
235  return true;
236 
237  // ==================================================================
238  // Otherwise, it was just opened now, we must send the
239  // ** initialization commands **
240  // and put the laser in continuous measuring mode:
241  // ==================================================================
242  if (!m_skip_laser_config)
243  {
244  if (!LMS_setupSerialComms()) RET_ERROR("error");
245 
246  bool res;
247  for (int nTry=0;nTry<4;nTry++)
248  if (true==(res=LMS_sendMeasuringMode_cm_mm()))
249  break;
250 
251  if (!res) return false;
252 
253  for (int nTry=0;nTry<4;nTry++)
254  if (true==(res=LMS_startContinuousMode()))
255  break;
256 
257  return res;
258  }
259  else
260  {
261  // Skip setup:
262  return true;
263  }
264  }
265  catch(std::exception &e)
266  {
267  std::string s = "[CSickLaserSerial] Error trying to open SICK at port ";
268  s+= e.what();
269  if (err_msg) *err_msg=s;
270  MRPT_LOG_ERROR(s);
271  return false;
272  }
273 }
274 
275 /*-------------------------------------------------------------
276  waitContinuousSampleFrame
277 -------------------------------------------------------------*/
279  vector<float> &out_ranges_meters,
280  unsigned char &LMS_status,
281  bool &is_mm_mode )
282 {
283  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
284  ASSERTMSG_(COM!=NULL,"No I/O channel bound to this object");
285 
286  size_t nRead,nBytesToRead;
287  size_t nFrameBytes = 0;
288  size_t lengthField;
289  unsigned char buf[2000];
290  buf[2]=buf[3]=buf[4]=0;
291 
292  while ( nFrameBytes < (lengthField=( 6 + (buf[2] | (buf[3] << 8))) ) )
293  {
294  if (lengthField>800)
295  {
296  cout << "#";
297  nFrameBytes = 0; // No es cabecera de trama correcta
298  buf[2]=buf[3]=0;
299  }
300 
301  if (nFrameBytes<4)
302  nBytesToRead = 1;
303  else
304  nBytesToRead = (lengthField) - nFrameBytes;
305 
306  try
307  {
308  nRead = COM->Read( buf+nFrameBytes,nBytesToRead );
309  }
310  catch (std::exception &e)
311  {
312  // Disconnected?
313  MRPT_LOG_ERROR_FMT("[CSickLaserSerial::waitContinuousSampleFrame] Disconnecting due to comms error: %s\n", e.what());
314  //m_usbConnection->Close();
315  return false;
316  }
317 
318  if ( !nRead && !nFrameBytes )
319  return false;
320 
321  if (nRead<nBytesToRead)
323 
324  // Lectura OK:
325  // Era la primera?
326  if (nFrameBytes>1 || (!nFrameBytes && buf[0]==0x02) || (nFrameBytes==1 && buf[1]==0x80))
327  {
328  nFrameBytes+=nRead;
329  }
330  else
331  {
332  nFrameBytes = 0; // No es cabecera de trama correcta
333  buf[2]=buf[3]=0;
334  //cerr << "."; //"[CSickLaserSerial] Skipping non-header..." << endl;
335  }
336  }
337 
338  // Frame received
339  // --------------------------------------------------------------------------
340  // | STX | ADDR | L1 | L2 | COM | INF1 | INF2 | DATA | STA | CRC1 | CRC2 |
341  // --------------------------------------------------------------------------
342 
343  // Trama completa:
344  // Checkear que el byte de comando es 0xB0:
345  if ( buf[4]!=0xB0 ) return false;
346 
347  // GET FRAME INFO
348  int info = buf[5] | (buf[6] << 8); // Little Endian
349  int n_points = info & 0x01FF;
350  is_mm_mode = 0 != ((info & 0xC000) >> 14); // 0x00: cm 0x01: mm
351 
352  out_ranges_meters.resize(n_points);
353 
354  // Copiar rangos:
355  short mask = is_mm_mode ? 0x7FFF : 0x1FFF;
356  float meters_scale = is_mm_mode ? 0.001f : 0.01f;
357 
358  for (int i=0;i<n_points;i++)
359  out_ranges_meters[i] = ( (buf[7+i*2] | (buf[8+i*2] << 8)) & mask ) * meters_scale;
360 
361  // Status
362  LMS_status = buf[lengthField-3];
363 
364  // CRC:
365  uint16_t CRC = mrpt::utils::compute_CRC16(buf,lengthField-2, CRC16_GEN_POL);
366  uint16_t CRC_packet = buf[lengthField-2] | ( buf[lengthField-1] << 8);
367  if (CRC_packet!=CRC)
368  {
369  cerr << format("[CSickLaserSerial::waitContinuousSampleFrame] bad CRC len=%u nptns=%u: %i != %i", unsigned(lengthField),unsigned(n_points), CRC_packet, CRC) << endl;
370  return false; // Bad CRC
371  }
372 
373  // All OK
374  return true;
375 }
376 
377 
378 /*-------------------------------------------------------------
379  initialize
380 -------------------------------------------------------------*/
382 {
383  string err_str;
385  if (!tryToOpenComms(&err_str))
386  {
387  cerr << err_str << endl;
388  throw std::logic_error(err_str);
389  }
390 }
391 
392 /*-----------------------------------------------------------------
393  Assures laser is connected and operating at 38400, in
394  its case returns true.
395  -----------------------------------------------------------------*/
397 {
398  ASSERT_(m_com_baudRate==9600 || m_com_baudRate==38400 || m_com_baudRate==500000);
399 
400  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
401  if (COM==NULL) return true;
402 
403  int detected_rate = 0;
404  for (size_t reps=0;!detected_rate && reps<m_nTries_connect;reps++)
405  {
406  m_nTries_current=reps;
407 
408  int rates[] = {0, 9600,38400,500000};
409 
410  // Try first the desired rate to speed up the process, just in case
411  // the laser is already setup from a previous run:
412  rates[0] = m_com_baudRate;
413 
414  detected_rate = 0;
415 
416  for (size_t i=0;!detected_rate && i<sizeof(rates)/sizeof(rates[0]);i++)
417  {
418  // Are we already receiving at 500k?
419  // ------------------------------------------------
420  COM->setConfig( rates[i] );
421 
422  LMS_endContinuousMode(); // Stop continuous mode.
423  mrpt::system::sleep(100);
424  COM->purgeBuffers();
425 
426  for (int nTry=0;nTry<4 && !detected_rate;nTry++)
427  {
428  COM->purgeBuffers();
429  // Ask for the laser status at the current rate:
430  if ( LMS_statusQuery() )
431  {
432  detected_rate = rates[i];
433  break;
434  }
436  } // for tries
437  // There is no link, or the baudrate is wrong...
438  }
439 
440  // Try again in a while:
441  if (!detected_rate && reps!=(m_nTries_connect-1))
442  mrpt::system::sleep(5000);
443  }
444 
445  // Are we connected at the right rate?
446  if (detected_rate==m_com_baudRate)
447  return true;
448 
449  // Switch to the desired rate now
450  if ( !this->LMS_setupBaudrate(m_com_baudRate) ) RET_ERROR("error");
451 
452  // Check response is OK:
453  if (!(m_received_frame_buffer[2]==0x03 && m_received_frame_buffer[4]==0xA0 && m_received_frame_buffer[6]==0x10))
454  return false;
455 
457  COM->purgeBuffers();
458 
459  // Wait...
460  mrpt::system::sleep(500);
461 
462  // And check comms at the new baud rate:
463  return LMS_statusQuery();
464 }
465 
466 
467 /*-----------------------------------------------------------------
468  Query to LMS a baudrate change command.
469  Returns true if response is read ok.
470  -----------------------------------------------------------------*/
472 {
473  ASSERT_(m_stream);
474 
475  uint8_t cmd[4];
476  cmd[0] = 0x20;
477  switch (baud)
478  {
479  case 9600: cmd[1]=0x42; break;
480  case 19200: cmd[1]=0x41; break;
481  case 38400: cmd[1]=0x40; break;
482  case 500000:cmd[1]=0x48; break;
483  default:
484  THROW_EXCEPTION("Invalid baud rate value");
485  }
486 
487  uint16_t cmd_len = 2;
488 
489  if (!SendCommandToSICK(cmd,cmd_len)) return false;
490  return LMS_waitIncomingFrame(500);
491 }
492 
493 
494 /*-----------------------------------------------------------------
495  Query to LMS a status query.
496  Returns true if response is read ok.
497  -----------------------------------------------------------------*/
499 {
500  ASSERT_(m_stream);
501 
502  uint8_t cmd[1];
503  cmd[0] = 0x31;
504  uint16_t cmd_len = 1;
505 
506  if (!SendCommandToSICK(cmd,cmd_len)) return false;
507  return LMS_waitIncomingFrame(500);
508 }
509 
510 
511 // Returns false if timeout
513 {
514  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
515  ASSERT_(COM);
516 
517  uint8_t b = 0;
518  CTicTac tictac;
519  tictac.Tic();
520 
521  do
522  {
523  if ( COM->Read(&b,1) )
524  { // Byte rx:
525  if (b==0x06) return true;
526  }
527  }
528  while ( tictac.Tac()< timeout_ms*1e-3 );
529 
530  if (b==0x15)
531  RET_ERROR(format("NACK received."))
532  else if (b!=0)
533  RET_ERROR(format("Unexpected code received: 0x%02X",b))
534  else return false; //RET_ERROR("Timeout")
535 }
536 
537 
538 // Returns false if timeout
540 {
541  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
542  ASSERT_(COM);
543 
544  uint8_t b;
545  unsigned int nBytes=0;
546 
547  CTicTac tictac;
548  tictac.Tic();
549  const double maxTime = timeout*1e-3;
550 
551  while (nBytes<6 || (nBytes<(6U+m_received_frame_buffer[2]+(uint16_t)(m_received_frame_buffer[3]<<8))) )
552  {
553  if ( COM->Read(&b,1) )
554  {
555  // First byte must be STX:
556  if (nBytes>1 || (!nBytes && b==0x02) || (nBytes==1 && b==0x80))
557  {
558  // Store in frame:
559  m_received_frame_buffer[nBytes] = b;
560  nBytes++;
561  }
562  }
563  if (tictac.Tac()>=maxTime)
564  return false; // Timeout
565  }
566 
567  const uint16_t lengthField = m_received_frame_buffer[2] + (m_received_frame_buffer[3]<<8);
568  // Check len:
569  if (4U+lengthField+2U != nBytes)
570  {
571  printf("[CSickLaserSerial::LMS_waitIncomingFrame] Error: expected %u bytes, received %u\n",4U+lengthField+2U, nBytes);
572  return false;
573  }
574 
575  // Check CRC
577  uint16_t CRC_packet = m_received_frame_buffer[4+lengthField+0] | ( m_received_frame_buffer[4+lengthField+1] << 8);
578  if (CRC_packet!=CRC)
579  {
580  printf("[CSickLaserSerial::LMS_waitIncomingFrame] Error in CRC: rx: 0x%04X, computed: 0x%04X\n",CRC_packet,CRC);
581  return false;
582  }
583 
584 
585 #if 0
586  printf("RX: ");
587  for (unsigned int i=0;i<nBytes;i++)
588  printf("%02X ",m_received_frame_buffer[i]);
589  printf("\n");
590 #endif
591 
592  // OK
593  return true;
594 }
595 
596 
598 {
599  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
600  ASSERT_(COM);
601 
602  // **************************
603  // Send command: Switch to Installation mode
604  // **************************
605  uint8_t cmd[128]; // = {0x02,0x00,0x0A,0x00,0x20,0x00,0x53,0x49,0x43,0x4B,0x5F,0x4C,0x4D,0x53,0xBE,0xC5};
606  cmd[0] = 0x20; /* mode change command */
607  cmd[1] = 0x00; /* configuration mode */
608  cmd[2] = 0x53; // S - the password
609  cmd[3] = 0x49; // I
610  cmd[4] = 0x43; // C
611  cmd[5] = 0x4B; // K
612  cmd[6] = 0x5F; // _
613  cmd[7] = 0x4C; // L
614  cmd[8] = 0x4D; // M
615  cmd[9] = 0x53; // S
616 
617  uint16_t cmd_len = 10;
618  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("Error waiting ACK to installation mode");
619  if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error in response to installation mode");
620 
621  // Check response
622  if(!(m_received_frame_buffer[4] == 0xA0 && m_received_frame_buffer[5] == 0x00) )
623  RET_ERROR("Wrong response to installation mode");
624 
625  // **************************
626  // Request LMS Configuration
627  // **************************
628  cmd[0] = 0x74;
629  cmd_len = 1;
630 
631  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("No ACK to 0x74 (req. config)");
632  if (!LMS_waitIncomingFrame(500)) RET_ERROR("No answer to 0x74 (req. config)");
633 
634  // 2. Check response
635  if(m_received_frame_buffer[4] != 0xF4)
636  RET_ERROR("No expected 0xF4 in response to 0x74 (req. config)");
637 
638  // ***********************************************************************
639  // Configure 1/2: Measuremente Range, Measurement Unit, Master/Slave Role
640  // ***********************************************************************
641  // 4.a Modify some values...
642 
643  // Measuring mode: Measurement range 32m in mm mode, or 80m+reflectance info in cm mode.
644  // See page 98 in LMS2xx_list_datagrams.pdf.
645  m_received_frame_buffer[10] = this->m_mm_mode ? 0x06 : 0x00;
646  m_received_frame_buffer[11] = this->m_mm_mode ? 0x01 : 0x00;
647 
648  // 4.2 Build the output command
649  m_received_frame_buffer[1] = 0x00; // Address
650  m_received_frame_buffer[2] = 0x23; // Length (low byte)
651  m_received_frame_buffer[3] = 0x00; // Length (high byte)
652  m_received_frame_buffer[4] = 0x77; // Configure command
653 
654  memcpy(cmd, m_received_frame_buffer+4, 0x23);
655  cmd_len = 0x23;
656 
657  // 4.4 Send to the LMS
658  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("No ACK for config command (0x77)");
659  if(!LMS_waitIncomingFrame(600)) RET_ERROR("No answer for config command (0x77)");
660 
661  if(!(m_received_frame_buffer[4] == 0xF7 && m_received_frame_buffer[5] == 0x01))
662  RET_ERROR("Wrong answer for config command (0x77)");
663 
664  // **************************
665  // Switch to Monitoring mode
666  // **************************
667  cmd[0] = 0x20;
668  cmd[1] = 0x25;
669  cmd_len = 2;
670  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("No ACK for set monitoring mode");
671  if (!LMS_waitIncomingFrame(500)) RET_ERROR("No answer for set monitoring mode");
672 
673  if(!(m_received_frame_buffer[4] == 0xA0 && m_received_frame_buffer[5] == 0x00) )
674  RET_ERROR("Wrong answer for set monitoring mode");
675 
676  // All ok.
677  return true;
678 }
679 
680 /*-----------------------------------------------------------------
681  Start continuous mode measurements.
682  Returns true if response is read ok.
683  -----------------------------------------------------------------*/
685 {
686  ASSERT_(m_scans_FOV==100 || m_scans_FOV==180);
687  ASSERT_(m_scans_res==25 || m_scans_res==50 || m_scans_res==100);
688 
689  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
690  ASSERT_(COM);
691 
692  uint8_t cmd[40];
693 
694  // Config angle/resolution
695  cmd[0] = 0x3B;
696  cmd[1] = m_scans_FOV;
697  cmd[2] = 0x00;
698  cmd[3] = m_scans_res; // 25,50 or 100 - 1/100th of deg
699  cmd[4] = 0x00;
700  uint16_t cmd_len = 5;
701  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("Error waiting ack for change angle/resolution");
702  if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error waiting answer for change angle/resolution");
703 
704  // Start continuous mode:
705  cmd[0] = 0x20;
706  cmd[1] = 0x24;
707  cmd_len = 2;
708  if (!SendCommandToSICK(cmd,cmd_len)) RET_ERROR("Error waiting ack for start scanning");
709  if (!LMS_waitIncomingFrame(500)) RET_ERROR("Error waiting answer for start scanning");
710 
711  return true;
712 }
713 
715 {
716  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
717  ASSERT_(COM);
718 
719  uint8_t cmd[40];
720 
721  // End continuous mode:
722  cmd[0] = 0x20;
723  cmd[1] = 0x25;
724  uint16_t cmd_len = 2;
725  if (!SendCommandToSICK(cmd,cmd_len)) return false;
726  return LMS_waitIncomingFrame(50);
727 }
728 
730 {
731  uint8_t cmd_full[1024];
732  ASSERT_(sizeof(cmd_full)>cmd_len+4U+2U);
733 
734  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
735  ASSERT_(COM);
736 
737  // Create header
738  cmd_full[0] = 0x02; // STX
739  cmd_full[1] = 0; // ADDR
740  cmd_full[2] = cmd_len & 0xFF;
741  cmd_full[3] = cmd_len >> 8;
742 
743  memcpy(cmd_full+4,cmd,cmd_len);
744 
745  const uint16_t crc = mrpt::utils::compute_CRC16(cmd_full, 4+cmd_len, CRC16_GEN_POL);
746  cmd_full[4+cmd_len+0] = crc & 0xFF;
747  cmd_full[4+cmd_len+1] = crc >> 8;
748 
749  const size_t toWrite = 4+cmd_len+2;
750 
751 #if 0
752  printf("TX: ");
753  for (unsigned int i=0;i<toWrite;i++)
754  printf("%02X ",cmd_full[i]);
755  printf("\n");
756 #endif
757 
758  const int NTRIES = 3;
759 
760  for (int k=0;k<NTRIES;k++)
761  {
762  if (toWrite!=COM->Write( cmd_full, toWrite ))
763  {
764  cout << "[CSickLaserSerial::SendCommandToSICK] Error writing data to serial port." << endl;
765  return false;
766  }
768  if (LMS_waitACK(50)) return true;
770  }
771 
772  return false;
773 }
774 
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
GLenum GLint GLuint mask
Definition: glext.h:3888
bool LMS_sendMeasuringMode_cm_mm()
Returns false on error.
CSerialPort * m_mySerialPort
Will be !=NULL only if I created it, so I must destroy it at the end.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool isOpen() const
Returns if port has been correctly open.
unsigned __int16 uint16_t
Definition: rptypes.h:46
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
utils::CStream * m_stream
The I/O channel (will be NULL if not bound).
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool turnOff()
Disables the scanning mode (in this class this has no effect).
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
bool LMS_statusQuery()
Send a status query and wait for the answer. Return true on OK.
#define THROW_EXCEPTION(msg)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
Contains classes for various device interfaces.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
size_t Write(const void *Buffer, size_t Count)
Implements the virtual method responsible for writing to the stream.
STL namespace.
int m_com_baudRate
Baudrate: 9600, 38400, 500000.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
GLdouble s
Definition: glext.h:3602
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
bool LMS_setupSerialComms()
Assures laser is connected and operating at 38400, in its case returns true.
#define RET_ERROR(msg)
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
bool LMS_setupBaudrate(int baud)
Send a command to change the LMS comms baudrate, return true if ACK is OK. baud can be: 9600...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initialize()
Set-up communication with the laser.
std::string m_com_port
If set to non-empty, the serial port will be attempted to be opened automatically when this class is ...
#define M_PIf
bool m_skip_laser_config
If true, doesn&#39;t send the initialization commands to the laser and go straight to capturing...
bool waitContinuousSampleFrame(std::vector< float > &ranges, unsigned char &LMS_status, bool &is_mm_mode)
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLubyte GLubyte b
Definition: glext.h:5575
uint16_t BASE_IMPEXP compute_CRC16(const std::vector< uint8_t > &data, const uint16_t gen_pol=0x8005)
Computes the CRC16 checksum of a block of data.
Definition: crc.cpp:22
bool LMS_waitIncomingFrame(uint16_t timeout)
Returns false if timeout.
mrpt::math::TPose3D m_sensorPose
The sensor 6D pose:
#define DEG2RAD
void loadCommonParams(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
GLsizei const GLchar ** string
Definition: glext.h:3919
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
virtual ~CSickLaserSerial()
Destructor.
void setSerialPortName(const std::string &COM_name)
Sets the serial port to open (it is an error to try to change this while open yet).
Definition: CSerialPort.h:65
int m_scans_res
1/100th of deg: 100, 50 or 25
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool SendCommandToSICK(const uint8_t *cmd, const uint16_t cmd_len)
Send header+command-data+crc and waits for ACK. Return false on error.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void purgeBuffers()
Purge tx and rx buffers.
void open()
Open the port.
Definition: CSerialPort.cpp:87
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser sca...
backing_store_ptr info
Definition: jmemsys.h:170
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
#define ASSERT_(f)
unsigned int m_nTries_connect
Default = 1.
bool turnOn()
Enables the scanning mode (in this class this has no effect).
#define MRPT_LOG_ERROR(_STRING)
GLuint res
Definition: glext.h:6298
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
bool tryToOpenComms(std::string *err_msg=NULL)
Tries to open the com port and setup all the LMS protocol. Returns true if OK or already open...
void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)
Specific laser scanner "software drivers" must process here new data from the I/O stream...
#define ASSERTMSG_(f, __ERROR_MSG)
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool LMS_waitACK(uint16_t timeout_ms)
Returns false if timeout.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
size_t Read(void *Buffer, size_t Count)
Implements the virtual method responsible for reading from the stream - Unlike CStream::ReadBuffer, this method will not raise an exception on zero bytes read, as long as there is not any fatal error in the communications.
void setScanRangeValidity(const size_t i, const bool val)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019