はじめに
初投稿です！
よろしくお願いします！
環境
・メインOS : Windows10 pro
・VirtualBOX + Ubuntu16.04
・Unity2017.4.4f1
・ROS kinetic
なお，Ubuntuは仮想環境じゃなくても同じローカルにあるならば通信可能
ROS#(ros-sharp)とは
・siemensが作成したオープンソースソフトウェア
・.NETアプリケーション，特にUnityからROSと通信するためのもの
以下のURLにアップされています．
https://github.com/siemens/ros-sharp
ROS#の導入自体の説明はまたの機会に
私は以下のサイトを参考にさせていただきました．
OculusGoをROSと通信させる
https://qiita.com/Spritaro/items/5eb99b2fcdc26a3816ce
今回やること
・ROS側からpublishされたPointCloud2をUnity側でsubscribeする.
前提
・ROS側からPointCloud2がpublishされているものとする（ROSは初心者のためほぼ触れません）
・ROS#の基本的な設定は終わっているものとする
・Unityの経験少々
PointCloud2をUnity側で扱えるかを確認
ROS#のGithubをのぞいて
Libraries\RosBridgeClient\Messages\Sensor
の中にPointCloud2.csがあることを確認
/*
© Siemens AG, 2017-2018
Author: Dr. Martin Bischoff (martin.bischoff@siemens.com)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
<http://www.apache.org/licenses/LICENSE-2.0>.
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
using Newtonsoft.Json;
namespace RosSharp.RosBridgeClient.Messages.Sensor
{
public class PointCloud2 : Message
{
[JsonIgnore]
public const string RosMessageName = "sensor_msgs/PointCloud2";
public Standard.Header header;
public int height;
public int width;
public PointField[] fields;
public bool is_bigendian;
public int point_step;
public int row_step;
public byte[] data;
public bool is_dense;
public PointCloud2()
{
header = new Standard.Header();
height = 0;
width = 0;
fields = new PointField[0];
is_bigendian = false;
point_step = 0;
row_step = 0;
is_dense = false;
data = new byte[0];
}
}
}
これを見てみるとROSの定義しているPCL2とほぼ同じく扱えることが分かります．
基本的にintやbool型のものが多いので，そのあたりは通常通り扱えます．
今回私が引っかかったのは，byte型で送られてくる点群の座標の扱いです．
これは，Unity側のスクリプトで解決していきます．
Unityのスクリプトの準備
Assets\RosSharp\Scripts\RosCommunicationに以下のファイルを追加
using System;
using System.Collections;
using System.Collections.Generic;
using RosSharp.RosBridgeClient.Messages.Sensor;
using UnityEngine;
using UnityEngine.UI;
namespace RosSharp.RosBridgeClient
{
[RequireComponent(typeof(RosConnector))]
public class PointCloudSubscriber : Subscriber<Messages.Sensor.PointCloud2>
{
private byte[] byteArray;
private bool isMessageReceived = false;
private int size;
private Vector3[] pcl;
int width;
int height;
int row_step;
int point_step;
protected override void Start()
{
base.Start();
}
public void Update()
{
if (isMessageReceived)
{
Coordinates();
isMessageReceived = false;
}
}
protected override void ReceiveMessage(PointCloud2 message)
{
size = message.data.GetLength(0);
int i=0;
byteArray = new byte[size];
foreach (byte temp in message.data)
{
byteArray[i] = temp; //byte型を取得
i++;
}
width = message.width;
height = message.height;
row_step = message.row_step;
point_step = message.point_step;
//取得したデータを出力
Debug.Log("width" + message.width);
Debug.Log("height" + message.height);
Debug.Log("row_step" + message.row_step);
Debug.Log("point_step" + message.point_step);
size = size/point_step;
}
//点群の座標を変換
void Coordinates()
{
pcl = new Vector3[size];
//この部分でbyte型をfloatに変換
for (int n = 0; n < size; n++)
{
int x_posi = n * point_step + 0;
int y_posi = n * point_step + 4;
int z_posi = n * point_step + 8;
float x = BitConverter.ToSingle(byteArray, x_posi);
float y = BitConverter.ToSingle(byteArray, y_posi);
float z = BitConverter.ToSingle(byteArray, z_posi);
pcl[n] = new Vector3(y, z, x);
//結果を出力
Debug.Log("pclCoordinates:x=" + pcl[n].x + ",y=" + pcl[n].y + ",z=" + pcl[n].y);
}
}
}
}
point_stepが一つの座標に用いられているbyteの長さを示し,X,Y,Zはそれぞれ4byteずつ格納されている．
ちなみに，0・4・8という数字はfloat型で保存されていることが分かっているのでこの値です．
したがって，n番目の点群の座標情報はbyte型配列の以下の位置からスタートしている．
x_posi = n * point_step + 0;
y_posi = n * point_step + 4;
z_posi = n * point_step + 8;
最後にBitConverter.ToSingleというメソッドを使ってfloatに変換したらオッケーです．
float x = BitConverter.ToSingle(byteArray, x_posi);
float y = BitConverter.ToSingle(byteArray, y_posi);
float z = BitConverter.ToSingle(byteArray, z_posi);
おわりに
今回は，初投稿ということもありかなり雑にまとめてしまった感があります．
本当は，初期設定のやり方から新しい型の追加とかまでやりたかった．．．
取り急ぎPCL2でひかかったところだけまとめた感じです．
何か間違えているところやご指摘などありましたら，よろしくお願いします．