PolygonArrayメッセージを使う ROS

はじめに

3次元の点のリストのROSメッセージに geometry_msgs/Polygon というmsgがあります。
メッセージの例は以下の様になります。

points: 
  - 
    x: 235.61074829101562
    y: 235.62962341308594
    z: 0.0
  - 
    x: 393.2195739746094
    y: 235.62974548339844
    z: 0.0
  - 
    x: 550.6107788085938
    y: 235.62962341308594
    z: 0.0
  - 
    x: 235.60983276367188
    y: 393.2467041015625
    z: 0.0
  - 
    x: 393.2194519042969
    y: 393.2442321777344
    z: 0.0

これを更にリストにして使いたいと思い調べたところ jsk_recognition_msgs の PolygonArray というものが使えそうだと思い使い方を調べてまとめました。

PolygonArray

このメッセージの内部を調べてみると PolygonStamped のリストであることがわかりました。
(参考)messageの型

更に使い方として以下のようなサンプルプログラムがあり参考にしました。
(参考)sample program

使い方

最後に実際に使ってみます。

(環境) ros noetic, ubuntu20.04

まず、インストールします。

sudo apt install ros-noetic-jsk-recognition-msgs 

pythonでpub,subするサンプルプログラムは次のようになります。

次にサンプルを作成します。
publishする場合は次のようになります。

p_array_pub.py

import rospy
from jsk_recognition_msgs.msg import PolygonArray
from geometry_msgs.msg import Polygon, PolygonStamped, Point32

msg = PolygonArray()

p1 = PolygonStamped()
p1.polygon.points = [Point32(x=1, y=2, z=3),
                     Point32(x=1, y=2, z=3),
                     Point32(x=1, y=2, z=3)]

p2 = PolygonStamped()
p2.polygon.points = [Point32(x=4, y=5, z=6),
                     Point32(x=4, y=5, z=6),
                     Point32(x=4, y=5, z=6)]


msg.polygons = [p1,p2]

pub = rospy.Publisher('p_array', PolygonArray, queue_size=1)
rospy.init_node('pub')
r = rospy.Rate(1)
while not rospy.is_shutdown():
  pub.publish(msg)
  print("publish!")
  r.sleep()

Subscribeする場合は次のようになります。

p_array_sub.py

import rospy
from jsk_recognition_msgs.msg import PolygonArray

def callback(msg):
  print("*** callback ***")
  print(msg)

rospy.Subscriber('p_array', PolygonArray, callback)
rospy.init_node('sub')
rospy.spin()