RPLIDARをobnizで使う

このエントリーをはてなブックマークに追加

RPLIDARは低価格のレーザースキャナーです。
まるでレーダーみたいに周りの物体距離を360度取得できます。

obnizにつないでHTMLに描画してみました。

Program

<!-- HTML Example -->
<!DOCTYPE html>
<html>
<head>
  <meta charset="utf-8">
  <meta name="viewport" content="width=device-width, initial-scale=1">

  <link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/4.0.0/css/bootstrap.min.css" integrity="sha384-Gn5384xqQ1aoWXA+058RXPxPg6fy4IWvTNh0E263XmFcJlSAwiGgFAW/dAiS6JXm" crossorigin="anonymous">
  <script  src="https://code.jquery.com/jquery-3.2.1.min.js" integrity="sha256-hwg4gsxgFZhOsEEamdOYGBf13FyQuiTwlAQgxVSNgt4="  crossorigin="anonymous"></script>
  <script src="https://cdnjs.cloudflare.com/ajax/libs/popper.js/1.12.3/umd/popper.min.js" integrity="sha384-vFJXuSJphROIrBnz7yo7oB41mKfc8JzQZiCq4NCceLEaO4IHwicKwpJf9c9IpFgh" crossorigin="anonymous"></script>
  <script src="https://maxcdn.bootstrapcdn.com/bootstrap/4.0.0/js/bootstrap.min.js" integrity="sha384-JZR6Spejh4U02d8jOt6vLEHfe/JQGiRRSQQxSfFWpi1MquVdAyjUar5+76PVCmYl" crossorigin="anonymous"></script>

  <style>
  textarea {
    width: 100%;
  }
  </style>

  <script src="https://unpkg.com/obniz@0.1.50/obniz.js"></script>
</head>

<body>
<div id="obniz-debug"></div>
<br>
<div class="text-center">
  <h1> RPLIDAR </h1>
</div>

<div id="info"> </div>
<div id="dot" class="btn btn-primary">Dot Mode</div>
<div id="line" class="btn btn-primary">Line Mode</div>
<canvas id="canvas" width="700" height="700"></canvas>

<script>

/* This will be over written on obniz.io webapp page */
var obniz = new Obniz("OBNIZ_ID_HERE");

obniz.onconnect = async () => {
  obniz.reset();

  obniz.io0.output(0);
  obniz.io4.output(1);
  obniz.io5.output(1);
  obniz.io9.output(0);
  obniz.io10.output(1);
  obniz.io11.output(1);
  
  var mode = "dot"
  $("#dot").click(function(){
    mode ="dot";
  })
  $("#line").click(function(){
    mode ="line";
  })

  const uart = obniz.getFreeUart();
  uart.start({tx: 1, rx: 2, baud:115200, drive:"5v"});
  
  const info = await getDeviceInfo(uart);
  $("#info").text("firmwareVersion:" + info.firmwareVersion)

  const health = await getHealth(uart)
  if (health.status != 0) {
    throw new Error("invalid status " + health.status);
  }

  await startScan(uart);

  var canvas = $("#canvas")[0];
  var ctx = ctx = canvas.getContext('2d', { alpha: false });
  ctx.fillStyle = 'red';
  ctx.strokeStyle = '#A00';

  obniz.repeat(async function(){
    var datas = await getNextData(uart);
    ctx.clearRect(0, 0, canvas.width, canvas.height);
    for (var i=0; i<datas.length; i++) {
      // console.log(""+datas[i].angle+" - "+datas[i].distance + " Q:"+datas[i].quality)
      var x = canvas.width/2;
      var y = canvas.height/2;
      x += datas[i].distance * Math.cos(datas[i].angle /360.0 * 2 * Math.PI) / 10
      y += datas[i].distance * Math.sin(datas[i].angle /360.0 * 2 * Math.PI) / 10
	  if (mode == "line") {
        ctx.beginPath();
        ctx.moveTo(canvas.width/2, canvas.height/2);
        ctx.lineTo(x, y);
        ctx.stroke();
      } else {
        ctx.fillRect(x, y, 1, 1);
      }
    }
    await obniz.wait(1);
  }, 1);
}

async function reset(uart) {
  const RPLIDAR_CMD_RESET = 0x40
  sendCommand(uart, RPLIDAR_CMD_RESET); // no payload
  var payload = await getFrame(uart);
}

let _parseOffset = 0;
let _savedDatas = [];
async function getNextData(uart) {
  let datas = [];
  let r = _savedDatas;
  _savedDatas = [];
  while(true) {
    r = r.concat( uart.readBytes());
    for (var i=0; i<r.length; i++) {
      switch(_parseOffset) {
        case 0:
          datas.push({});
          datas[datas.length-1].sync_quality = r[i];
          break;
        case 1:
          datas[datas.length-1].angle_q6_checkbit = r[i];
          break;
        case 2:
          datas[datas.length-1].angle_q6_checkbit |= r[i] << 8;
          break;
        case 3:
          datas[datas.length-1].distance_q2 = r[i];
          break;
        case 4:
          datas[datas.length-1].distance_q2 |= r[i] << 8;

          const last = datas[datas.length-1];

          last.isStart = (last.sync_quality & 0x01 != 0) ? true : false;
          last.quality = last.quality >> 2;
          last.angle = (last.angle_q6_checkbit >> 1) / 64;
          last.check = last.angle_q6_checkbit & 0x01;
          last.distance = last.distance_q2 /4;

          break;
      }
      if (++_parseOffset > 4) {
        _parseOffset = 0;
        if (datas[datas.length-1].isStart) {
          if (datas.length != 1) {
            r.splice(0, i + 1);
            _savedDatas = r;
            return datas;
          }
        }
      }
    }
    await obniz.wait(1);
  }
}

async function startScan(uart) {

  const RPLIDAR_CMD_SCAN  = 0x20;
  const RPLIDAR_CMD_FORCE_SCAN = 0x21;
  sendCommand(uart, RPLIDAR_CMD_SCAN); // no payload
  var payload = await getFrame(uart);
  // type should be 129
}

async function startExpressScan(uart) {
  const RPLIDAR_CMD_EXPRESS_SCAN = 0x82
  const fixedAngle = false;
  let workingMode = fixedAngle ? 1 : 0;
  var payload = [];
  var index = 0;
  payload.push(workingMode);
  payload.push(0);
  payload.push(0);
  payload.push(0);
  payload.push(0);
  sendCommand(uart, RPLIDAR_CMD_EXPRESS_SCAN, payload); // no payload
  var payload = await getFrame(uart);
  // type should be RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82(130)
}

async function getHealth(uart) {
  const RPLIDAR_CMD_GET_DEVICE_HEALTH = 0x52

  sendCommand(uart, RPLIDAR_CMD_GET_DEVICE_HEALTH); // no payload

  var payload = await getFrame(uart);

  var health = {};
  health.status     = payload.splice(0, 1);
  health.error_code = parseInt(payload.splice(0, 1));
  health.error_code |= payload.splice(0, 1) << 8;
  return health;
}

async function getExpressSupporeted(uart) {
  const RPLIDAR_CMD_GET_SAMPLERATE = 0x59;
  sendCommand(uart, RPLIDAR_CMD_GET_SAMPLERATE); // no payload
  var payload = await getFrame(uart);
}

async function getDeviceInfo(uart) {
  const RPLIDAR_CMD_GET_DEVICE_INFO = 0x50

  sendCommand(uart, RPLIDAR_CMD_GET_DEVICE_INFO); // no payload

  var payload = await getFrame(uart);
  
  var deviceInfo = {};
  deviceInfo.model = payload.splice(0, 1);
  deviceInfo.firmwareVersion = parseInt(payload.splice(0, 1));
  deviceInfo.firmwareVersion |= payload.splice(0, 1) << 8;
  deviceInfo.hardware_version = payload.splice(0, 1);
  deviceInfo.serialNumber = payload;
  return deviceInfo;
}

function sendCommand(uart, command, payload) {
  const SYNC = 0xA5;
  const RPLIDAR_CMD_SYNC_BYTE = 0xA5
  
  var data = [SYNC, command]
  if (payload) {
    var checksum = 0;
    checksum ^= RPLIDAR_CMD_SYNC_BYTE;
    checksum ^= command;
    checksum ^= (payload.length & 0xFF);
    for (let pos = 0; pos < payload.length; ++pos) {
        checksum ^= payload[pos];
    }
    data.push(payload.length);
    data = data.concat(payload);
    data.push(checksum);
  }

  uart.send(data);
}

  // const RPLIDAR_ANS_TYPE_DEVINFO  = 0x4;
  // const RPLIDAR_ANS_TYPE_DEVHEALTH = 0x6;

async function getFrame(uart) {
  const RPLIDAR_ANS_SYNC_BYTE1 = 0xA5;
  const RPLIDAR_ANS_SYNC_BYTE2 = 0x5A;
  let recv = [];
  let waiting = true

  // parse header
  while(waiting) {
    const r = uart.readBytes();
    recv = recv.concat(r);
    for (var i=0; i<recv.length-1; i++) {
      if (recv[i] == RPLIDAR_ANS_SYNC_BYTE1 && recv[i+1] == RPLIDAR_ANS_SYNC_BYTE2) {
        // remain is 7?
        if(recv.length - (i+2) >= 5) {
          recv.splice(0, i+2);
          waiting = false;
          break;
        }
      }
    }
    await obniz.wait(10);
  }
  var subType = parseInt(recv.splice(0, 1));
  subType |=  recv.splice(0, 1) << (8*1);
  subType |=  recv.splice(0, 1) << (8*2);
  subType |=  recv.splice(0, 1) << (8*3);
  var type =  recv.splice(0, 1);

  const RPLIDAR_ANS_HEADER_SIZE_MASK = 0x3FFFFFFF;

  var size = subType & RPLIDAR_ANS_HEADER_SIZE_MASK;

  console.log(`type${type} size${size}`);

  // parse payload
  while(true) {
    const r = uart.readBytes();
    recv = recv.concat(r);
    if (recv.length >= size) {
      break;
    }
    await obniz.wait(10);
  }
  return recv.splice(0, size);
}
    
</script>
</body>
</html>

今すぐ実行

HTMLがブラウザで開かれて実行されます。