SpresenseをROSでUnityにつなぐ

今日の目的

SpresenseのL1Sでの測位結果をUnityで取得する。

SpresenseでL1S測位

Spresenseは、Sonyが出しているマイコンボードです。Arduinoや、Sony独自組み込みOS環境でプログラムできます。
特徴的な機能として、GPSがマイコンボードに組み込まれていて、さらに準天頂衛星みちびきのL1Sという補正信号を受信して、より精度の高い測位をすることができます。
（他にも特徴的な機能はあり、全体として非常に面白いマイコンボードですが、ここではGNSSについてだけ書きます。）
まずは、Arduino開発環境で、L1Sを取得してシリアルに送出するプログラムを作成します。

といっても、よくできたサンプルコードを公式が公開しているので、今回はこれを少しだけ変更して使います。

変更点は、以下の使う衛星の設定箇所です。

/* Set this parameter depending on your current region. */
static enum ParamSat satType =  eSatGpsQz1cQz1S;

これで、GPSとQZSSのL1C/AとL1Sを受信して測位するようになります。

Raspberry Pi Zero WH でシリアル受信して、ROSでメッセージ送出

上記のプログラムをArduino IDEからコンパイルして書き込むと、USBシリアルに測位関連のメッセージが流れてきます。

pi@raspberrypi:~/ros_catkin_ws $ cat /dev/ttyUSB0
SpGnss : begin in
SpGnss : begin out
SpGnss : start in
  mode = COLD_START
SpGnss : start out
Gnss setup OK
1980/01/06 00:00:01.000498, numSat: 0, No-Fix, No Position
1980/01/06 00:00:02.000519, numSat: 1, No-Fix, No Position
1980/01/06 00:00:03.000508, numSat: 1, No-Fix, No Position
1980/01/06 00:00:04.000498, numSat: 1, No-Fix, No Position

こんな感じです。
実際に使うときは、NMEAあたりのフォーマットで送るのがいいかなと思いますが、今回はこのままROS経由でUnityに持って行ってみます。

先回作成したROSのPublisherのプログラムを以下のように変えました。

ros_gnss_test_node.cpp

#include <stdio.h>
#include <stdlib.h>
#include "ros/ros.h"
#include "ros_gnss_test/MsgGNSS.h"

int main(int argc, char **argv)
{
  FILE *fp;
  char buffer[1024];

  fp = fopen("/dev/ttyUSB0", "r");
  if(fp == NULL) {
    printf("ERROR fopen");
    exit(0);
  }

  ros::init(argc, argv, "ros_gnss_test");
  ros::NodeHandle nh;

  ros::Publisher ros_gnss_test_pub = nh.advertise<ros_gnss_test::MsgGNSS>("ros_gnss_msg", 100);
  ros::Rate loop_rate(10);

  ros_gnss_test::MsgGNSS msg;

  while(ros::ok())
  {
    for(int i = 0; i< 1024; i++) buffer[i] = 0;
    if(fgets(buffer, 1024, fp) != NULL){
      msg.data = buffer;
      msg.stamp = ros::Time::now();

      ROS_INFO("send msg = %s", msg.data);

      ros_gnss_test_pub.publish(msg);

      loop_rate.sleep();
    }
  }

  fclose(fp);

  return 0;
}

UnityでROSメッセージを受け取る

前回のプロジェクトをそのまま使って、メッセージを受け取ってみました。

雑ですが、GPSの情報をUnityに持ってくることができました。
ちゃんと作ればいろいろ便利そうです。