ROS：点群の座標変換（Python）

この記事ではROSで、ステレオカメラ等からのPointCloud2型のtopicを座標変換する方法について説明します．これを使うとカメラ画像から検出した物体の座標をカメラローカルからマップグローバルに変換できます．

環境

Ubuntu18.04
ROS(Melodic)

準備

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

方法

ROSではlink同士の座標変換を行うときには主にtfと呼ばれるライブラリを使用します．これを用いるとsourceとtargetを指定するだけで座標変換をすることができます．点群座標はPointStamped型を使用します．
以下の方法でPointCloud2型のtopicを座標変換します．今回の場合はcamera_link->mapです．用途によって変更してください．

def transform_position(ps_list, source_link='map', target_link='camera_link'):
    listener = tf.TransformListener()
    ps = PointStamped() 
    ps.header.frame_id = target_link
    ps.header.stamp = rospy.Time()
    ps.point.x = ps_list[0] # カメラローカルのx座標
    ps.point.y = ps_list[1] # カメラローカルのy座標
    ps.point.z = ps_list[2] # カメラローカルのz座標
    try:
        listener.waitForTransform(source_link, target_link, rospy.Time(), rospy.Duration(10))
        tf_ps = listener.transformPoint(source_link, ps)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
        raise e
    return tf_ps # PointStamped型で返却されることに注意

参考

http://wiki.ros.org/ja/tf
http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PointStamped.html