ROS：サービスを使った点群の座標変換

この記事ではROSのサービスを使って点群等を座標変換する方法について説明します．

環境

Ubuntu18.04
ROS(Melodic)
Python 2.7.x

前準備

本記事では事前に静的な座標変換を行っていることを前提としています．
各センサを起動するlaunchファイルにstatic_transform_publisherを用いて座標変換を記述してください．

概要

ROSではlink同士の座標変換を行うときには主にtfと呼ばれるライブラリを使用します．これを用いるとsourceとtargetを指定するだけで座標変換をすることができます．前回,Pythonで任意の一点を座標変換する方法について投稿しましたが，これを全部の点に使用するとかなりの時間がかかります．また，tf2を使うとdoTransformでPython内で変換することができますが，これもまた時間がかかります．そこで，ROSのサービスを使ってC++内でpcl_rosを用いて座標変換をしてPythonで変換結果を受けとります．サービスの説明についてはGoogleしてください．

本準備

以下のパッケージをダウンロードしてください．

$ cd ~/catkin_ws/src/
$ git clone https://github.com/HHorimoto/transform_pointcloud_srv.git
$ cd ~/catkin_ws
$ catkin_make

使い方

transform_pointcloud_srvを起動してください．これが変換するサービス側です．

$ rosrun transform_pointcloud_srv transform_pointcloud_srv

サンプルプログラミング（client_py.py）を起動してください．これが変換結果を受け取るクライアント側です．.

このプログラムのトピック名及びリンク名を変える必要があります．

$ rosrun transform_pointcloud_srv client_py.py

サンプルプログラムの説明

初めに，サービスの名前を見てください．---で区切られた上2つはリクエストの引数で下1つはリスポンスの結果です．

$ roscd transform_pointcloud_srv/srv
$ cat TransformPointcloud.srv
std_msgs/String topic_name # 1st Request argument
std_msgs/String target_frame # 2nd Request argument
---
sensor_msgs/PointCloud2 cloud_out # Response value

TransformPointcloudサービスをインポートしてください.

from transform_pointcloud_srv.srv import TransformPointcloud

サービスが起動するまで待ちます.

rospy.wait_for_service('transform_pointcloud')

サービスを引数を指定して呼び出して結果を受けてください.

# call
client = rospy.ServiceProxy('transform_pointcloud', TransformPointcloud)
frame_msg = std_msgs.msg.String()
frame_msg.data = "base_link"
topic_name = std_msgs.msg.String()
topic_name.data = "/camera/depth/color/points"
# get
resp1 = client(topic_name, frame_msg)

あなたの用途に合わせて変換結果を以下のように使用してください.

# print PointCloud data's frame_id, height, and width.
rospy.loginfo("info : frame_id = %s, height = %d, width = %d", resp1.cloud_out.header.frame_id, resp1.cloud_out.height, resp1.cloud_out.width)

参考

https://qiita.com/yakato_jun/items/091f36308b0662110537