Main MRPT website > C++ reference for MRPT 1.5.6
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
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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.
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.
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)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
size_t Write(const void *Buffer, size_t Count)
Implements the virtual method responsible for writing to the stream.
int m_com_baudRate
Baudrate: 9600, 38400, 500000.
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,...)
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
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...
void initialize()
Set-up communication with the laser.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLdouble s
Definition: glew.h:1295
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'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
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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)...
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
virtual ~CSickLaserSerial()
Destructor.
GLuint res
Definition: glew.h:7143
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.
bool isOpen() const
Returns if port has been correctly open.
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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
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)
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
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...
GLenum GLint GLuint mask
Definition: glew.h:1752
#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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018