※ ROSの表記について :
ROS1／ROS2について、文脈的にどちらでも当てはまる場合は単に「ROS」と表記しています。
はじめに
本記事は全4回のシリーズ記事の第3回です
第1回 基本部分の作成
第2回 画像データの受信と表示
第3回 点群データの受信と表示
第4回 Webアプリケーション側からのROS2情報送信
前回は「画像データの受信と表示」として、ROS2を起動するサーバ側システム (以降は単に「サーバ」と表記) での画像の送信と、受け取りやすくするための変換を行い、Next.jsによるクライアント側Webアプリケーション (以降は単に「Webアプリケーション」と表記) 側で受け取って表示させてみました。
今回は、更にサーバ側からの点群データ送信処理を追加し、Webアプリケーション側で点群データの表示もできるようにしてみます。
点群データ送信の方針
点群データは sensor_msgs/PointCloud と sensor_msgs/PointCloud2 が定義されています。
2種類のデータはそれぞれ一長一短がありますが、より自由な表現ができる sensor_msgs/PointCloud2 データの方がよく使われている印象を受けます。
今回はサーバ側で sensor_msgs/PointCloud2 のデータを送信し、Webアプリケーション側で受け取ったものをWebGLを使って表示させてみることにします。
実装
サーバ側
サーバ側では、点群データ sensor_msgs/PointCloud2 を送信するノードを作ります。
PointCloud2データは自由度が高いのが良いのですが、その分フィールド定義の設定が必要です。
( サーバ側ディレクトリ )
$ cd visualize_sample_server
( エディタを開いて編集 )
$ vim scripts/point_cloud_sender_node.pyソース詳細
import math
import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2, PointField
### Node name
NODE_NAME = 'point_cloud_sender'
### Topic name
TOPIC_NAME = '/point_cloud'
### Timer period
TIMER_PERIOD = 0.25 # 0.25秒おきに呼び出し
### 1フレームに生成する点群サイズ
SIZE = 3000
### PointCloud2 msg用パラメータ
ROS_DTYPE = PointField.FLOAT32 # データ型
ITEM_SIZE = np.dtype(np.float32).itemsize # データ型に即したサイズ (float32 = 4byte)
# Point field (型定義) ... (x, y, z) のみ
POINT_FIELDS = [PointField(name = n, offset = i * ITEM_SIZE, datatype = ROS_DTYPE, count = 1) for i, n in enumerate('xyz')]
### Pointcloud sender node
class PointCloudSenderNode(Node) :
def __init__(self) :
super().__init__(NODE_NAME)
# Initialize
self.publisher = self.create_publisher(PointCloud2, TOPIC_NAME, 10)
# 点群生成の基準半径を広げたり縮めたりする
self.r = 5.0
self.step = 0.25
self.unit = 1.0
self.r_max = 10.0
self.r_min = 1.0
# Timer
self.timer = self.create_timer(TIMER_PERIOD, self.on_timer)
def on_timer(self) :
# Construct header
header = Header(frame_id = 'map')
header.stamp = self.get_clock().now().to_msg()
# Construct point cloud message
msg = PointCloud2()
msg.header = header
msg.height = 1
msg.width = SIZE
msg.fields = POINT_FIELDS
msg.is_dense = False
msg.is_bigendian = False
msg.point_step = ITEM_SIZE * 3 # 3 = xyz
msg.row_step = ITEM_SIZE * 3 * SIZE
# Random points
points = self.create_random_circle_points(SIZE, r = self.r)
msg.data = points.astype(np.float32).tobytes()
# Publish
self.publisher.publish(msg)
# Step radius
self.r += self.step * self.unit
if self.r >= self.r_max :
self.r = self.r_max
self.unit = -1.0
elif self.r <= self.r_min :
self.r = self.r_min
self.unit = 1.0
def create_random_circle_points(self, size, r = 1.0, sigma = 1.0, zmin = 1.0, zmax = 2.0) :
'''
XY平面上に、半径rの円を基準にした分散sigmaの正規分布上ランダムの点を打つ
更に、高さはzmin〜zmaxの範囲のランダムにした3次元の点をsize個生成する。
'''
rnd = np.random.default_rng()
# XY平面上の円形点群
rs = np.random.normal(r, sigma, size)
thetas = rnd.random(size) * 2.0 * math.pi # 0° 〜 360°
xs = rs * np.cos(thetas)
ys = rs * np.sin(thetas)
# Z
zs = (zmax - zmin) * rnd.random(size) + zmin
return np.vstack([xs, ys, zs]).T
def main(args = None):
# Initialize rclpy
print("Start point cloud sender node.")
rclpy.init(args = args)
# Create node
node = PointCloudSenderNode()
# Spin
try :
rclpy.spin(node)
except KeyboardInterrupt : # Ctrl + C
pass
print("Shutdown point cloud sender node.")
rclpy.shutdown()
if __name__ == '__main__':
main()ランダムな点群データを生成しますが、それなりの動きが見えないと面白くないので、
『円形を基準にして、少しずつ大きくなったり小さくなったりする』動きを付けます。
必要なノードの作成が完了したので、後は setup.py と launch ファイルの呼び出しの設定を行います。
( setup.py の編集 )
$ vim setup.pyソース詳細
entry_points={
'console_scripts': [
'sample_sender_node = scripts.sample_sender_node:main',
'image_sender_node = scripts.image_sender_node:main',
'image_converter_node = scripts.image_converter_node:main',
'point_cloud_sender_node = scripts.point_cloud_sender_node:main', # <-- 追加
],
},( launch ファイルの編集 )
$ vim launch/execute.launch.py ソース詳細
import os
import launch
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description() :
ld = LaunchDescription()
# ROS bridge
node_rosbridge = Node(
package = 'rosbridge_server',
executable = 'rosbridge_websocket',
arguments = [
'--address', '0.0.0.0', '--port', '8888',
],
)
ld.add_action(node_rosbridge)
# Image sender node
node_image_sender = Node(
package = 'visualize_sample_server',
executable = 'image_sender_node',
name = 'image_sender',
)
ld.add_action(node_image_sender)
# Image converter node
node_image_converter = Node(
package = 'visualize_sample_server',
executable = 'image_converter_node',
name = 'image_converter',
)
ld.add_action(node_image_converter)
# Point cloud sender node # <-- 追加
node_point_cloud_sender = Node(
package = 'visualize_sample_server',
executable = 'point_cloud_sender_node',
name = 'point_cloud_sender',
)
ld.add_action(node_point_cloud_sender)
# Sample text sender node
node_sample_text_sender = Node(
package = 'visualize_sample_server',
executable = 'sample_sender_node',
name = 'sample_text_sender',
)
ld.add_action(node_sample_text_sender)
return ldWebアプリケーション側
Webアプリケーション側では、点群を読み込んで表示するPointCloudViewerを作成します。
Three.jsを使ってWebGLの表示を行います。
( Webアプリケーション側ディレクトリ )
$ cd visualize_sample_client
( エディタを開いて作成 )
$ vim pages/components/point_cloud_viewer.tsxソース詳細
import React, { forwardRef, useImperativeHandle, useRef, useState } from 'react';
// Three.js
import * as THREE from 'three';
import { Canvas } from '@react-three/fiber';
import { Vector3 } from 'three';
import { useFrame } from '@react-three/fiber';
import { OrbitControls } from '@react-three/drei'; // カメラ処理
import { Point, Points } from '@react-three/drei'; // Particle
//////////////////////////////////////////////////
/// Settings ///
//////////////////////////////////////////////////
// 背景色
const BACKGROUND_COLOR = '#050505';
// 表示パーティクル最大値
// NOTE: これより大きい点群が入ってきた場合はそれ以上を読み込まない。
const PARTICLES_MAX = 25000;
// パーティクル (1つ分の) サイズ
const PARTICLE_SIZE = 0.12;
// パーティクルの色
const PARTICLE_COLOR = '#B3A675';
//////////////////////////////////////////////////
/// Particles ///
//////////////////////////////////////////////////
// Viewerの点群表示部
const ParticlesViewer = forwardRef((props, ref) => {
// 上位モジュールから呼び出される処理
useImperativeHandle(ref, () => ({
// 点群更新
update: (newPoints: Vector3[]) => {
setPoints(newPoints.slice(0, PARTICLES_MAX)); // 最大値までに制限をかける
}
}));
// 描画グループに対するref
const gRef = useRef<THREE.Group>(null!);
// 点群
const [points, setPoints] = useState<Vector3[]>([]);
// Three.js フレーム毎処理
// useFrame(() => {
// // 実験用に、描画グループに回転をかける
// gRef.current.rotation.z += 0.001;
// });
// 表示
return (
<>
<group>
{/* 点群 */}
<group ref={gRef}>
<Points limit={PARTICLES_MAX}>
<pointsMaterial vertexColors size={PARTICLE_SIZE} />
{points.map((p, i) => (
<Point key={i}
position={[p.x, p.y, p.z]}
color={PARTICLE_COLOR} />
))}
</Points>
</group>
</group>
</>
);
});
//////////////////////////////////////////////////
/// Point cloud viewer ///
//////////////////////////////////////////////////
const PointCloudViewer = forwardRef((props, ref) => {
// 上位モジュールから呼び出される処理
useImperativeHandle(ref, () => ({
// 点群更新
update: (newPoints: Vector3[]) => {
particlesRef.current.update(newPoints); // 点群表示モジュールにそのまま投げる
}
}));
// 点群表示モジュールへのref
const particlesRef = useRef<any>(null!);
// 表示
return (
<>
<Canvas style={{width: "99%", height: "45vh"}}>
{/* 背景色 */}
<color attach="background" args={[BACKGROUND_COLOR]} />
{/* 環境光 */}
<ambientLight />
{/* OrbitControls ... カメラ操作 */}
<OrbitControls target-z={1} />
{/* 表示確認用の軸表示 */}
<axesHelper args={[6]} />
{/* 点群表示 */}
<ParticlesViewer ref={particlesRef} />
</Canvas>
</>
);
});
// Export settings
PointCloudViewer.displayName = 'PointCloudViewer';
export default PointCloudViewer;後はRosOperatorの方で /point_cloud トピックを購読できるようにし、Viewer側のつなぎ込みを行います。
( RosOperator 編集 )
$ vim libs/ros_operation.tsxソース詳細
import React, { forwardRef, useImperativeHandle, useEffect, useRef, useState } from 'react';
import * as ROSLIB from 'roslib';
import { Vector3 } from 'three'; // 受信したPointCloudをVector3に変換
///////////////////////////////////////////////////
/// Settings ///
///////////////////////////////////////////////////
// ROS接続URL
// TODO: 実際使用するURLへの更新を行う。
const ROS_CONNECTION_URL = 'ws://127.0.0.1:8888'
// Test string topic name
const TOPIC_NAME_TEST_STR = '/test/message';
// Image (string) topic name
const TOPIC_NAME_IMAGE = '/image/converted';
// Point cloud topic name
const TOPIC_NAME_POINT_CLOUD = '/point_cloud';
///////////////////////////////////////////////////
/// Interfaces ///
///////////////////////////////////////////////////
// String 型
export interface String {
data: string;
}
// PointCloud2 型 (フィールド)
export interface PointField {
name: string;
offset: number;
datatype: number;
count: number;
}
// PointCloud2 型 (データ)
export interface PointCloud2 {
height: number;
width: number;
fields: PointField[];
point_step: number;
row_step: number;
data: string; // base64 encoder されているので、decode して利用する
}
///////////////////////////////////////////////////
/// ROS Operator ///
///////////////////////////////////////////////////
const RosOperator = forwardRef((props, ref) => {
// 上位モジュールから呼び出される処理
useImperativeHandle(ref, () => ({
// ROSとの接続
connect: () => {
connectToROS();
},
}));
// ROSLIB object
const [ros, setRosObject] = useState<ROSLIB.Ros | null>(null);
// Test string topic listener
const [testStrListener, setTestStrListener] = useState<ROSLIB.Topic | null>(null);
// Image topic listener
// NOTE: 画像データはStringとして受信する
const [imageListener, setImageListener] = useState<ROSLIB.Topic | null>(null);
// Point cloud topic listener
const [pointCloudListener, setPointCloudListener] = useState<ROSLIB.Topic | null>(null);
// Connect to ROS ... ROSとの接続を開始する
const connectToROS = () => {
// 接続開始
console.log('ROS Operator: Try connection...');
props.setRosConnected?.(false); // NOTE: 上位モジュールのROS接続確認フラグをOFF
// Initialize ROS connection object
setRosObject(new ROSLIB.Ros({url:ROS_CONNECTION_URL}));
};
// ROS オブジェクト更新時に Listener & Sender を更新
useEffect(() => {
// 初回表示時は ROS オブジェクトが null なのでスキップ
if (ros === null) { return; }
// 接続完了検知 or エラー検知
// NOTE: 検知時に上位モジュールにフラグ通知
ros.on('connection', () => {
console.log('ROS Operator: ROS connected.');
props.setRosConnected?.(true);
});
ros.on('error', (err) => {
console.log('ROS Operator: ROS connection error, ', err);
props.setRosConnected?.(false);
});
ros.on('close', () => {
console.log('ROS Operator: ROS connection closed.');
props.setRosConnected?.(false);
});
// Test string topic listener
setTestStrListener(
new ROSLIB.Topic({
ros: ros,
name: TOPIC_NAME_TEST_STR,
messageType: 'std_msgs/String',
})
);
// Image (string) topic listener
setImageListener(
new ROSLIB.Topic({
ros: ros,
name: TOPIC_NAME_IMAGE,
messageType: 'std_msgs/String',
})
);
// Point cloud topic listener
setPointCloudListener(
new ROSLIB.Topic({
ros: ros,
name: TOPIC_NAME_POINT_CLOUD,
messageType: 'sensor_msgs/PointCloud2',
})
);
}, [ros]);
// testStrListener更新時 に subscribe 設定
useEffect(() => {
// 初回表示時はオブジェクトが空なのでスキップ
if (testStrListener === null) { return; }
// Test string topic msg subscription event
testStrListener.subscribe((msg: ROSLIB.Message) => {
// 上位モジュールへ通知
props.updateTestStr?.((msg as String).data);
});
}, [testStrListener]);
// imageListener更新時に subscribe 設定
useEffect(() => {
// 初回表示時はオブジェクトが空なのでスキップ
if (imageListener === null) { return; }
// Image topic msg subscription event
imageListener.subscribe((msg: ROSLIB.Message) => {
// 上位モジュールへ通知
props.updateImage?.((msg as String).data);
});
}, [imageListener]);
// pointCloudListener更新時に subscribe 設定
useEffect(() => {
// 初回表示時はオブジェクトが空なのでスキップ
if (pointCloudListener === null) { return; }
// Point cloud topic msg subscription event
// NOTE: 本来はfield情報を読み込んでdataのパースを行う必要があるが、
// 今回は1つ分の情報が (x, y, z) となっている前提で処理を行う。
pointCloudListener.subscribe((msg: ROSLIB.Message) => {
// base64データのデコードし、バイナリ形式に変換
const pcMsg:PointCloud2 = msg as PointCloud2;
const width = pcMsg.width;
const pointStep = pcMsg.point_step;
const decoded = atob(pcMsg.data); // base64されているデータのデコード
const data:number[] = decoded.split('').map(l => l.charCodeAt(0)); // uint8配列に変換
console.log('ROS operator received PointCloud2 msg, size = ', width);
// Vector3の配列に変換
const vecs:Vector3[] = [];
for (let n = 0; n < width; n++) {
// バイナリ情報のインデックスを取得
const bPos = n * pointStep + 0;
const xPos = n * pointStep + 4;
const yPos = n * pointStep + 8;
const zPos = n * pointStep + 12;
// バイナリ → 数値変換して追加
const x = new Float32Array(new Uint8Array(data.slice(bPos, xPos)).buffer)[0];
const y = new Float32Array(new Uint8Array(data.slice(xPos, yPos)).buffer)[0];
const z = new Float32Array(new Uint8Array(data.slice(yPos, zPos)).buffer)[0];
vecs.push(new Vector3(x, y, z));
}
// 上位モジュールへ通知
props.updatePointCloud?.(vecs);
});
}, [pointCloudListener]);
// 空要素
return (
<></>
);
});
RosOperator.displayName = 'RosOperator';
export default RosOperator;( 主となるViewer部の編集 )
$ vim pages/viewer.tsxソース詳細
// React
import React, { useRef, useState } from 'react';
// Bootstrap
import { Container, Row, Col } from 'react-bootstrap';
import { Button, Form } from 'react-bootstrap';
import 'bootstrap/dist/css/bootstrap.min.css';
// Three.js
import { Vector3 } from 'three';
// ROS Operator
import RosOperator from '../libs/ros_operation';
// Components
import ImageStringViewer from './components/image_string_viewer';
import PointCloudViewer from'./components/point_cloud_viewer';
//////////////////////////////////////////////////
/// Viewer ///
//////////////////////////////////////////////////
const Viewer = () => {
// ROS operator ref
const rosOpRef = useRef<any>(null!);
// Image string viewer ref
const imageStringViewerRef = useRef<any>(null!);
// Point cloud viewer ref
const pointCloudViewerRef = useRef<any>(null!);
// ROS 接続状態 (ROS Operatorにて更新)
const [rosConnected, setRosConnected] = useState<boolean>(false);
// Test string
const [testStr, setTestStr] = useState<string>('');
// ROS接続開始
const startConnect = () => {
rosOpRef.current.connect();
};
// ROS側の画像データ更新
// NOTE: ROS OperatorよりImage string viewerに伝達
const updateImage = (data:string) => {
imageStringViewerRef.current.update(data);
};
// ROS側の点群データ更新
// NOTE: ROS OperatorよりPoint cloud viewerに伝達
const updatePointCloud = (data:Vector3[]) => {
pointCloudViewerRef.current.update(data);
};
// 表示
return (
<>
{/* ROS Operator */}
<RosOperator ref={rosOpRef}
setRosConnected={setRosConnected}
updateImage={updateImage}
updatePointCloud={updatePointCloud}
updateTestStr={setTestStr}
/>
<Container>
{/* Title */}
<Row>
<Col>
<h1>ROS2 data visualization sample</h1>
</Col>
</Row>
{/* Header ... ROS接続ボタンおよび接続状態確認 */}
<Row>
<Col>
<Button
variant={!rosConnected ? "primary" : "secondary"}
disabled={rosConnected}
onClick={!rosConnected ? startConnect : null}
>
ROS接続開始
</Button>
<h2>ROS接続状態 ... {rosConnected ? 'ON' : 'OFF'}</h2>
</Col>
</Row>
{/* Viewer ... 表示部 */}
<Row>
{/* Image viewer */}
<Col>
<ImageStringViewer ref={imageStringViewerRef} width={600} />
</Col>
{/* Point cloud viewer (using Three.js) */}
<Col>
<PointCloudViewer ref={pointCloudViewerRef} />
</Col>
</Row>
{/* Logger ... ログ表示等 */}
<Row>
<Col>
<h2>ログ表示</h2>
<div>{testStr}</div>
</Col>
</Row>
</Container>
</>
);
};
export default Viewer;実行
それぞれの機能が完成したので、実行してみます。
サーバ側 :
$ source ./install/setup.sh
$ ros2 launch visualize_sample_server execute.launch.pyWebアプリケーション側 (npmデバッグ実行) :
$ PORT=8000 npm run devブラウザでWebアプリケーションにアクセス( ここでは http://127.0.0.1:8000/viewer ) し、
「ROS接続開始」ボタンでサーバ側と繋ぐと、送られてきた点群データが順次表示されることが確認できます。
ここまでで、前回の画像の受信の処理に引き続き、点群の受信の処理も作成することができました。
まとめと次回
今回は第3回として、サーバ側での点群データの送信、Webアプリケーション側での表示を行いました。
普段はLiDAR等センサからそのまま得ている点群データ。適当なランダムデータとしても、1から作るのは例があまりないようで、始めは情報が見つからずに大変でした。それでもROSとしてしっかり定義されているので、正しく設定すれば自由に作って送信することは可能でした。
点群の受信 & 表示側はThree.jsを始めとしてWebGLを扱うライブラリが充実しているので、現在の技術で実現しようと思うと難しくはないですね。
一通りの受信処理が作成できたので、最終回の次回はこれまでとは逆に、Webアプリケーション側からのROS2データの送信を試してみます。