rviz publish point 

`rviz`の`Publish Point`が3DアプリケーションのUIとして便利だなと思い使用例をまとめてみることにしました.


# 環境

ubuntu20.04
ros noetic

# rviz上にポリゴン生成

publish pointを送信するためにはrviz上にオブジェクトが必要です.
今回は`jsk_recognition_msgs/PolygonArray`を使用します(poincloudや2d mapでも大丈夫です)

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

msg = PolygonArray()

p1 = PolygonStamped()
p1.header.frame_id = "map"
p1.polygon.points = [Point32(x=10, y=10, z=0),
                     Point32(x=10, y=-10, z=0),
                     Point32(x=-10, y=-10, z=0),
                     Point32(x=-10, y=10, z=0)
                     ]


msg.polygons = [p1]

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

上記プログラムを実行し,rvizを起動します.
そして,rviz内では下記内容を実施します.

- `Fixed Frame`を`map`にする
- PolygonArrayのtopicを表示
- PolygonArrayのトピック名を指定
- PolygonArrayのonly borderのチェックを外す(ポリゴンの内部を埋めるため)

![Screenshot from 2022-11-22 17-09-34.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/9535bae7-d9d1-2ea0-ac52-6cc74a1e679c.png)

これで,rviz内に写真のようなポリゴンが生成されます.

![Screenshot from 2022-11-22 17-07-40.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/6848d89b-c78e-3deb-b549-c4f313fafb5a.png)

# cmd line

次にrostopic echoで値を見てみます.
rostopic echoで`clicked_point`を出力して,
そして`Publish Point`ボタンをクリックし先程生成したポリゴン内部の好きな点をクリックします.

![Screenshot from 2022-11-22 16-50-41.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/85f10645-c46c-f128-f630-e8a401af6d38.png)

そうすると値を受信できます.

```
rostopic echo /clicked_point
header: 
  seq: 1
  stamp: 
    secs: 1669104777
    nsecs: 820443680
  frame_id: "map"
point: 
  x: -1.3679866790771484
  y: -3.3910675048828125
  z: -0.001163482666015625
```

# python subscriber

次はpythonプログラムから値を受信します.
以下のようなプログラムを実行し,`Publish Point`を実行します.

```clicked_point_sub.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PointStamped

def callback(data):
  print(data)
    
def listener():

  rospy.init_node('click_point_sub', anonymous=True)

  rospy.Subscriber("clicked_point", PointStamped, callback)

  rospy.spin()
      
if __name__ == '__main__':
    listener()
```

以下のようにポイントを取得できました.

```
python3 clicked_points_sub.py 
header: 
  seq: 9
  stamp: 
    secs: 1669105726
    nsecs: 105534679
  frame_id: "map"
point: 
  x: 6.440458297729492
  y: 3.813863754272461
  z: 0.003772735595703125
```

# /clicked_pointsを貯める+PointCloud2でpublish

次は,クリックした位置をためて,溜まっている点をpointcloud2で配信します.

```clicked_points_store.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import *
from std_msgs.msg import *
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2

HEADER = Header(frame_id='map')

FIELDS = [
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    PointField(name='intensity', offset=8, datatype=PointField.FLOAT32, count=1),
#    PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
    ]

POINTS = []

def callback(data):
  global POINTS
  p = data.point
  POINTS.append([p.x,p.y,p.z,len(POINTS)])
  print("== points ==")
  print(POINTS)
    
rospy.init_node('click_point_sub', anonymous=True)
rospy.Subscriber("clicked_point", PointStamped, callback)
pc_pub = rospy.Publisher("/points", PointCloud2, queue_size=10)

r = rospy.Rate(10)

while not rospy.is_shutdown():

  point_cloud = pc2.create_cloud(HEADER, FIELDS, POINTS)
  pc_pub.publish(point_cloud)

  r.sleep()
```

<iframe width="560" height="315" src="https://www.youtube.com/embed/bZtvcWQG7ik" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

↑ 実行結果

# 豆知識
 

## 連続で`Poublish Point`を使用する方法
- rviz > Panels から `Tool Properties`にチェックを入れ表示
- `Tool Properties` > `Publish Point` の`Single click`のチェックを外す