linux - シリアルポート SICK LMS200 からデータを読み取るには?

Question

シリアル ポート経由で SICK LMS200 Lidar スキャナからデータを読み取りたいと考えています。シリアル ポートは /dev/ttyUSB0 として表示されます。そこからデータを取得する方法がわかりません。

ツールボックスのマニュアルに示されているように、手動インストールを使用して、Ubuntu 14.04.3 LTS にThe Sick LIDAR Matlab/C++ Toolboxをインストールしようとしました。

ファイルを展開します。

$tar -xzvf sicktoolbox-1.0.1.tar.gz

それが ./configure を実行した後に得られるものです（役立つかどうかはわかりません）

me@MyPC:~/smetiste/Sick/sicktoolbox-1.0.1$ ./configure 
checking for a BSD-compatible install... /usr/bin/install -c
checking whether build environment is sane... yes
checking for a thread-safe mkdir -p... /bin/mkdir -p
checking for gawk... gawk
checking whether make sets $(MAKE)... yes
checking for g++... g++
checking for C++ compiler default output file name... a.out
checking whether the C++ compiler works... yes
checking whether we are cross compiling... no
checking for suffix of executables... 
checking for suffix of object files... o
checking whether we are using the GNU C++ compiler... yes
checking whether g++ accepts -g... yes
checking for style of include used by make... GNU
checking dependency style of g++... gcc3
checking for gcc... gcc
checking whether we are using the GNU C compiler... yes
checking whether gcc accepts -g... yes
checking for gcc option to accept ISO C89... none needed
checking dependency style of gcc... gcc3
checking how to run the C preprocessor... gcc -E
checking for a BSD-compatible install... /usr/bin/install -c
checking whether ln -s works... yes
checking whether make sets $(MAKE)... (cached) yes
checking build system type... i686-pc-linux-gnu
checking host system type... i686-pc-linux-gnu
checking for a sed that does not truncate output... /bin/sed
checking for grep that handles long lines and -e... /bin/grep
checking for egrep... /bin/grep -E
checking for ld used by gcc... /usr/bin/ld
checking if the linker (/usr/bin/ld) is GNU ld... yes
checking for /usr/bin/ld option to reload object files... -r
checking for BSD-compatible nm... /usr/bin/nm -B
checking how to recognise dependent libraries... pass_all
checking for ANSI C header files... yes
checking for sys/types.h... yes
checking for sys/stat.h... yes
checking for stdlib.h... yes
checking for string.h... yes
checking for memory.h... yes
checking for strings.h... yes
checking for inttypes.h... yes
checking for stdint.h... yes
checking for unistd.h... yes
checking dlfcn.h usability... yes
checking dlfcn.h presence... yes
checking for dlfcn.h... yes
checking how to run the C++ preprocessor... g++ -E
checking for g77... no
checking for xlf... no
checking for f77... no
checking for frt... no
checking for pgf77... no
checking for cf77... no
checking for fort77... no
checking for fl32... no
checking for af77... no
checking for xlf90... no
checking for f90... no
checking for pgf90... no
checking for pghpf... no
checking for epcf90... no
checking for gfortran... no
checking for g95... no
checking for xlf95... no
checking for f95... no
checking for fort... no
checking for ifort... no
checking for ifc... no
checking for efc... no
checking for pgf95... no
checking for lf95... no
checking for ftn... no
checking whether we are using the GNU Fortran 77 compiler... no
checking whether  accepts -g... no
checking the maximum length of command line arguments... 32768
checking command to parse /usr/bin/nm -B output from gcc object... ok
checking for objdir... .libs
checking for ar... ar
checking for ranlib... ranlib
checking for strip... strip
checking if gcc supports -fno-rtti -fno-exceptions... no
checking for gcc option to produce PIC... -fPIC
checking if gcc PIC flag -fPIC works... yes
checking if gcc static flag -static works... yes
checking if gcc supports -c -o file.o... yes
checking whether the gcc linker (/usr/bin/ld) supports shared libraries... yes
checking whether -lc should be explicitly linked in... no
checking dynamic linker characteristics... GNU/Linux ld.so
checking how to hardcode library paths into programs... immediate
checking whether stripping libraries is possible... yes
checking if libtool supports shared libraries... yes
checking whether to build shared libraries... yes
checking whether to build static libraries... yes
configure: creating libtool
appending configuration tag "CXX" to libtool
checking for ld used by g++... /usr/bin/ld
checking if the linker (/usr/bin/ld) is GNU ld... yes
checking whether the g++ linker (/usr/bin/ld) supports shared libraries... yes
checking for g++ option to produce PIC... -fPIC
checking if g++ PIC flag -fPIC works... yes
checking if g++ static flag -static works... yes
checking if g++ supports -c -o file.o... yes
checking whether the g++ linker (/usr/bin/ld) supports shared libraries... yes
checking dynamic linker characteristics... GNU/Linux ld.so
checking how to hardcode library paths into programs... immediate
appending configuration tag "F77" to libtool
checking for gnuplot... no
configure: WARNING: *** gnuplot not found, some examples will not be built.
checking for doxygen... no
configure: WARNING: doxygen not found - will not generate any doxygen documentation
checking for perl... /usr/bin/perl
checking for the pthreads library -lpthreads... no
checking whether pthreads work without any flags... no
checking whether pthreads work with -Kthread... no
checking whether pthreads work with -kthread... no
checking for the pthreads library -llthread... no
checking whether pthreads work with -pthread... yes
checking for joinable pthread attribute... PTHREAD_CREATE_JOINABLE
checking if more special flags are required for pthreads... no
checking for ANSI C header files... (cached) yes
checking arpa/inet.h usability... yes
checking arpa/inet.h presence... yes
checking for arpa/inet.h... yes
checking fcntl.h usability... yes
checking fcntl.h presence... yes
checking for fcntl.h... yes
checking netinet/in.h usability... yes
checking netinet/in.h presence... yes
checking for netinet/in.h... yes
checking for string.h... (cached) yes
checking sys/ioctl.h usability... yes
checking sys/ioctl.h presence... yes
checking for sys/ioctl.h... yes
checking sys/socket.h usability... yes
checking sys/socket.h presence... yes
checking for sys/socket.h... yes
checking sys/time.h usability... yes
checking sys/time.h presence... yes
checking for sys/time.h... yes
checking termios.h usability... yes
checking termios.h presence... yes
checking for termios.h... yes
checking for unistd.h... (cached) yes
checking linux/serial.h usability... yes
checking linux/serial.h presence... yes
checking for linux/serial.h... yes
checking for stdbool.h that conforms to C99... yes
checking for _Bool... yes
checking for an ANSI C-conforming const... yes
checking for inline... inline
checking whether byte ordering is bigendian... no
checking whether time.h and sys/time.h may both be included... yes
checking for int16_t... yes
checking for uint16_t... yes
checking for uint32_t... yes
checking for uint8_t... yes
checking whether gcc needs -traditional... no
checking sys/select.h usability... yes
checking sys/select.h presence... yes
checking for sys/select.h... yes
checking for sys/socket.h... (cached) yes
checking types of arguments for select... int,fd_set *,struct timeval *
checking return type of signal handlers... void
checking for gettimeofday... yes
checking for memset... yes
checking for select... yes
checking for socket... yes
checking for strcasecmp... yes
configure: creating ./config.status
config.status: creating Makefile
config.status: creating c++/Makefile
config.status: creating c++/examples/Makefile
config.status: creating c++/examples/ld/Makefile
config.status: creating c++/examples/ld/ld_config/Makefile
config.status: creating c++/examples/ld/ld_config/src/Makefile
config.status: creating c++/examples/ld/ld_more_config/Makefile
config.status: creating c++/examples/ld/ld_more_config/src/Makefile
config.status: creating c++/examples/ld/ld_multi_sector/Makefile
config.status: creating c++/examples/ld/ld_multi_sector/src/Makefile
config.status: creating c++/examples/ld/ld_single_sector/Makefile
config.status: creating c++/examples/ld/ld_single_sector/src/Makefile
config.status: creating c++/examples/lms/Makefile
config.status: creating c++/examples/lms/lms_config/Makefile
config.status: creating c++/examples/lms/lms_config/src/Makefile
config.status: creating c++/examples/lms/lms_mean_values/Makefile
config.status: creating c++/examples/lms/lms_mean_values/src/Makefile
config.status: creating c++/examples/lms/lms_partial_scan/Makefile
config.status: creating c++/examples/lms/lms_partial_scan/src/Makefile
config.status: creating c++/examples/lms/lms_plot_values/Makefile
config.status: creating c++/examples/lms/lms_plot_values/src/Makefile
config.status: creating c++/examples/lms/lms_real_time_indices/Makefile
config.status: creating c++/examples/lms/lms_real_time_indices/src/Makefile
config.status: creating c++/examples/lms/lms_set_variant/Makefile
config.status: creating c++/examples/lms/lms_set_variant/src/Makefile
config.status: creating c++/examples/lms/lms_simple_app/Makefile
config.status: creating c++/examples/lms/lms_simple_app/src/Makefile
config.status: creating c++/examples/lms/lms_stream_range_and_reflect/Makefile
config.status: creating c++/examples/lms/lms_stream_range_and_reflect/src/Makefile
config.status: creating c++/examples/lms/lms_subrange/Makefile
config.status: creating c++/examples/lms/lms_subrange/src/Makefile
config.status: creating c++/drivers/Makefile
config.status: creating c++/drivers/ld/Makefile
config.status: creating c++/drivers/ld/sickld-1.0/Makefile
config.status: creating c++/drivers/lms/Makefile
config.status: creating c++/drivers/lms/sicklms-1.0/Makefile
config.status: creating c++/drivers/base/src/SickConfig.hh
config.status: executing depfiles commands

実行後$make、次のエラーが発生します。

me@ MyPC:~/smetiste/Sick/sicktoolbox-1.0.1$ make
Making all in c++
make[1]: Entering directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++'
Making all in drivers
make[2]: Entering directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers'
Making all in ld
make[3]: Entering directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers/ld'
Making all in sickld-1.0
make[4]: Entering directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers/ld/sickld-1.0'
/bin/sh ../../../../libtool --tag=CXX   --mode=compile g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src  -I../../../../c++/drivers/base/src   -g -O2 -MT SickLD.lo -MD -MP -MF .deps/SickLD.Tpo -c -o SickLD.lo SickLD.cc
mkdir .libs
 g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src -I../../../../c++/drivers/base/src -g -O2 -MT SickLD.lo -MD -MP -MF .deps/SickLD.Tpo -c SickLD.cc  -fPIC -DPIC -o .libs/SickLD.o
SickLD.cc: In member function 'void SickToolbox::SickLD::_flushTCPRecvBuffer()':
SickLD.cc:4137:35: warning: ignoring return value of 'ssize_t read(int, void*, size_t)', declared with attribute warn_unused_result [-Wunused-result]
        read(_sick_fd,&null_byte,1);
                                   ^
 g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src -I../../../../c++/drivers/base/src -g -O2 -MT SickLD.lo -MD -MP -MF .deps/SickLD.Tpo -c SickLD.cc -o SickLD.o >/dev/null 2>&1
mv -f .deps/SickLD.Tpo .deps/SickLD.Plo
/bin/sh ../../../../libtool --tag=CXX   --mode=compile g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src  -I../../../../c++/drivers/base/src   -g -O2 -MT SickLDMessage.lo -MD -MP -MF .deps/SickLDMessage.Tpo -c -o SickLDMessage.lo SickLDMessage.cc
 g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src -I../../../../c++/drivers/base/src -g -O2 -MT SickLDMessage.lo -MD -MP -MF .deps/SickLDMessage.Tpo -c SickLDMessage.cc  -fPIC -DPIC -o .libs/SickLDMessage.o
 g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src -I../../../../c++/drivers/base/src -g -O2 -MT SickLDMessage.lo -MD -MP -MF .deps/SickLDMessage.Tpo -c SickLDMessage.cc -o SickLDMessage.o >/dev/null 2>&1
mv -f .deps/SickLDMessage.Tpo .deps/SickLDMessage.Plo
/bin/sh ../../../../libtool --tag=CXX   --mode=compile g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src  -I../../../../c++/drivers/base/src   -g -O2 -MT SickLDBufferMonitor.lo -MD -MP -MF .deps/SickLDBufferMonitor.Tpo -c -o SickLDBufferMonitor.lo SickLDBufferMonitor.cc
 g++ -DHAVE_CONFIG_H -I. -I../../../../c++/drivers/base/src -I../../../../c++/drivers/base/src -g -O2 -MT SickLDBufferMonitor.lo -MD -MP -MF .deps/SickLDBufferMonitor.Tpo -c SickLDBufferMonitor.cc  -fPIC -DPIC -o .libs/SickLDBufferMonitor.o
In file included from SickLDBufferMonitor.hh:24:0,
                 from SickLDBufferMonitor.cc:24:
../../../../c++/drivers/base/src/SickBufferMonitor.hh: In member function 'void SickToolbox::SickBufferMonitor<SICK_MONITOR_CLASS, SICK_MSG_CLASS>::_readBytes(uint8_t*, int, unsigned int) const':
../../../../c++/drivers/base/src/SickBufferMonitor.hh:356:47: error: there are no arguments to 'getdtablesize' that depend on a template parameter, so a declaration of 'getdtablesize' must be available [-fpermissive]
       num_active_files = select(getdtablesize(),&file_desc_set,0,0,(timeout_value > 0) ? &timeout_val : 0);
                                               ^
../../../../c++/drivers/base/src/SickBufferMonitor.hh:356:47: note: (if you use '-fpermissive', G++ will accept your code, but allowing the use of an undeclared name is deprecated)
../../../../c++/drivers/base/src/SickBufferMonitor.hh:370:73: error: there are no arguments to 'read' that depend on a template parameter, so a declaration of 'read' must be available [-fpermissive]
      num_bytes_read = read(_sick_fd,&dest_buffer[total_num_bytes_read],1);
                                                                         ^
../../../../c++/drivers/base/src/SickBufferMonitor.hh: In static member function 'static void* SickToolbox::SickBufferMonitor<SICK_MONITOR_CLASS, SICK_MSG_CLASS>::_bufferMonitorThread(void*)':
../../../../c++/drivers/base/src/SickBufferMonitor.hh:456:18: error: there are no arguments to 'usleep' that depend on a template parameter, so a declaration of 'usleep' must be available [-fpermissive]
       usleep(1000);
                  ^
../../../../c++/drivers/base/src/SickBufferMonitor.hh: In instantiation of 'void SickToolbox::SickBufferMonitor<SICK_MONITOR_CLASS, SICK_MSG_CLASS>::_readBytes(uint8_t*, int, unsigned int) const [with SICK_MONITOR_CLASS = SickToolbox::SickLDBufferMonitor; SICK_MSG_CLASS = SickToolbox::SickLDMessage; uint8_t = unsigned char]':
SickLDBufferMonitor.cc:59:2:   required from here
../../../../c++/drivers/base/src/SickBufferMonitor.hh:356:47: error: 'getdtablesize' was not declared in this scope
       num_active_files = select(getdtablesize(),&file_desc_set,0,0,(timeout_value > 0) ? &timeout_val : 0);
                                               ^
../../../../c++/drivers/base/src/SickBufferMonitor.hh:370:73: error: 'read' was not declared in this scope
      num_bytes_read = read(_sick_fd,&dest_buffer[total_num_bytes_read],1);
                                                                         ^
make[4]: *** [SickLDBufferMonitor.lo] Error 1
make[4]: Leaving directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers/ld/sickld-1.0'
make[3]: *** [all-recursive] Error 1
make[3]: Leaving directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers/ld'
make[2]: *** [all-recursive] Error 1
make[2]: Leaving directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++/drivers'
make[1]: *** [all-recursive] Error 1
make[1]: Leaving directory `/home/me/smetiste/Sick/sicktoolbox-1.0.1/c++'
make: *** [all-recursive] Error 1
me@ MyPC:~/smetiste/Sick/sicktoolbox-1.0.1$ 

新しいリリースで「古い」パッケージを使用するのは難しいかもしれないと聞いたことがあります。しかし、それを機能させる機会はありますか？

内部メモリがないため、Linux でこのスキャナからデータを取得する方法はありますか?

Answer 1

SickBufferMonitor.hh と SickLIDAR.hh に unistd.h ヘッダー ファイルを含めると、問題が解決し、正常に終了します。

Answer 2

プロトコルは比較的単純で、十分に文書化されています。ドキュメントのサイズにひるまないでください。実際に使用するには、非常に小さなサブセット (コマンド10h,20hと応答90h,91h,92h,B0hから始める) が必要です。

以下は、開始するための小さな Python スクリプトです。

import binascii
import serial
import threading

# =============================================================================

SERIAL_PORT = 'COM6'
# NB: Different crystal, x8 multiplier
SERIAL_BAUDRATE = int(500000 * 14745600 / 16000000 / 8)
SERIAL_TIMEOUT = 1

# =============================================================================

def lms200_crc(data):
  crc16 = 0
  byte = [0,0]
  for c in data:
    byte[1] = byte[0]
    byte[0] = ord(c)

    if (crc16 & 0x8000):
      crc16 = (crc16 & 0x7FFF) << 1
      crc16 ^= 0x8005
    else:
      crc16 = crc16<< 1

    crc16 ^= (byte[1] << 8) | byte[0]

  return crc16

# =============================================================================

class LMS200Telegram(object):
  def __init__(self, data=None):
    if data:
      if len(data) < 7:
        raise RuntimeError("Invalid telegram size")
      if data[0] != '\x02':
        raise RuntimeError("Invalid STX")
      self.address = ord(data[1])
      self.length = ord(data[2]) + (ord(data[3]) * 256)
      self.payload = data[4:-2]
      self.crc = ord(data[-2]) + (ord(data[-1]) * 256)
    else:
      self.address = 0
      self.length = 0
      self.payload = ''
      self.crc = 0

  def generate(self):
    data = '\x02' + chr(self.address)
    self.length = len(self.payload)
    data += chr(self.length & 0xFF)
    data += chr((self.length >> 8) & 0xFF)
    data += self.payload
    self.crc = lms200_crc(data)
    data += chr(self.crc & 0xFF)
    data += chr((self.crc >> 8) & 0xFF)
    return data

class LMS200Command(object):
  def __init__(self, payload):
    if len(payload) < 1:
      raise RuntimeError("Invalid payload size")
    self.opcode = ord(payload[0])
    self.body = payload[1:]

class LMS200Response(object):
  def __init__(self, payload):
    if len(payload) < 2:
      raise RuntimeError("Invalid payload size")
    self.opcode = ord(payload[0])
    self.status = ord(payload[-1])
    self.body = payload[1:-1]

# =============================================================================

def cmd_reset_sensor():
  t = LMS200Telegram()
  t.payload = '\x10'
  return t

def cmd_start_measurement():
  t = LMS200Telegram()
  t.payload = '\x20\x20'
  return t

# =============================================================================

class LMS200Interface(object):
  def __init__(self, port, baudrate, timeout):
    self.port = port
    self.baudrate = baudrate
    self.timeout = timeout
    self.serial = None
    self.serial_rlock = threading.Lock()
    self.serial_wlock = threading.Lock()
    self.buffer = []

  def open_port(self):
    self.serial = serial.Serial(port=self.port
      , baudrate=self.baudrate
      , timeout=self.timeout)

  def close_port(self):
    self.serial.close()
    self.serial = None

  def process_data(self, data):
    self.buffer += data

  def write_telegram(self, t):
    self.serial.flush()
    with self.serial_wlock:
      self.serial.write(t.generate())
    with self.serial_rlock:
      while True:
        ack = self.serial.read(1)
        if len(ack) > 0:
          if ack == '\x06':
            return True
          elif ack == '\x15':
            return False
          else:
            raise RuntimeError("Invalid value received (neither ACK, nor NACK)")

  def worker(self):
    if not self.serial:
      raise RuntimeError("Serial port not initialized.")

    print "Reading serial port '%s'..." % self.port

    while(not self.stop_flag):
      data = self.serial.read(1024)
      if len(data) > 0:
        print "Got %d bytes." % len(data)
        self.process_data(data)

  def run(self):
    lms.open_port()
    self.thread = threading.Thread(target=self.worker)
    self.stop_flag = False
    self.thread.start()

  def stop(self):
    self.stop_flag = True
    self.thread.join()
    lms.close_port()

# =============================================================================

lms = LMS200Interface(SERIAL_PORT, SERIAL_BAUDRATE, SERIAL_TIMEOUT)

lms.run()

process_dataメソッドを次のように拡張する必要があります。

• 個々のテレグラム フレームを検出して分離します。
  • 最も簡単な方法は、STX バイトの発生を見つけることです。多くの場合、アドレスバイトは一定であるため、実際に検索することができます0x02 0x80。長さの検証を行うこともできます。次に、ペイロードを読み取って、CRC が一致するかどうかを確認します。
• テレグラムを特定したら、ペイロードを解析する必要があります。
  • コマンド/応答バイトでペイロード タイプを識別します。
  • Python structモジュールを使用してペイロードを解釈します。
• 必要に応じて測定データを処理してください。正確なフォームは、測定モードによって異なります。たとえば、基本測定モードでは、0.5 度刻みで 361 個の 16 ビット距離 (mm) の配列が得られます。最上位ビットに格納されているフラグをマスクし、エラー状態を表す特別な値を処理する必要があります。

これは、シリアル カードとの互換性の問題が発生したときにいくつかの基本的な通信テストを実行するためだけに作成されたため、スクリプトには実装していません。