Rvizの視点をプログラムから変更する

rvizの視点をプログラムから変えることができる`rviz_animated_view_controller`というパッケージの使い方を紹介します

https://github.com/ros-visualization/rviz_animated_view_controller

# 環境

```
ros-noetic
```

# install & build

```
cd ~/catkin_ws/src
git clone -b noetic-devel https://github.com/ros-visualization/rviz_animated_view_controller.git
cd ..
catkin build 
```

# run

```
roslaunch rviz_animated_view_controller demo.launch
```

![Peek 2022-08-20 20-18.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/34d8097a-315e-12b4-8bf1-16ff8d3a1f6d.gif)

実行するとrviの画面が自動で動きます!

# error

上記デモ実行時以下のエラー発生
```
  File "/opt/ros/noetic/lib/view_controller_msgs/SquareTest", line 42
    print "Publishing a message!"

```

`/opt/ros/noetic/lib/view_controller_msgs/SquareTest`の42行目をコメントアウト.編集時はroot権限で行う(sudoをつける)

# プログラム中身

画面をコントロールしているプログラムは`/opt/ros/noetic/lib/view_controller_msgs/SquareTest`でその中身は次のようになります。

```
#!/usr/bin/env python

import roslib
roslib.load_manifest("view_controller_msgs")

import rospy
from math import *
from view_controller_msgs.msg import CameraPlacement
from geometry_msgs.msg import Point, Vector3

rospy.init_node("camera_test", anonymous = True)

pub = rospy.Publisher("/rviz/camera_placement", CameraPlacement, queue_size = 1)

rate_float = 0.25
rate = rospy.Rate(rate_float)
rospy.sleep(1)

while not rospy.is_shutdown():

  #print "Top of loop!"

  t = rospy.get_time()
  cp = CameraPlacement()
  r = 10

  p = Point(r*cos(2*pi*t/20), r*sin(2*pi*t/20), 2)
  #p = Point(5,5,0)
  cp.eye.point = p
  cp.eye.header.frame_id = "base_link"

  #f = Point(0, 0, 2*cos(2*pi*t/5))
  f = Point(0, 0, 0)
  cp.focus.point = f
  cp.focus.header.frame_id = "base_link"

  up = Vector3(0, 0, 1)
  cp.up.vector = up
  cp.up.header.frame_id = "base_link"

  cp.time_from_start = rospy.Duration(1.0/rate_float)
#  print "Publishing a message!"
  pub.publish(cp)
  #print "Sleeping..."
  rate.sleep()
  #print "End of loop!"
```

このプログラムはwhile内で0.25Hz,4秒おきに以下のトピックを発信します。
```"/rviz/camera_placement", CameraPlacement, ```
そのメッセージの内容は以下のようになっていて,viewポイントの位置やフォーカス具合を,変数tによって変化させています。

```
  p = Point(r*cos(2*pi*t/20), r*sin(2*pi*t/20), 2)
  #p = Point(5,5,0)
  cp.eye.point = p
  cp.eye.header.frame_id = "base_link"
```

```
  #f = Point(0, 0, 2*cos(2*pi*t/5))
  f = Point(0, 0, 0)
  cp.focus.point = f
  cp.focus.header.frame_id = "base_link"
```

# guiから視点コントロール

先程,解説したプログラムのかわりｎ以下プログラムを使用すると,スライドバーからrvizの視点を制御できます.

```
import tkinter as tk
import math

import rospy
from view_controller_msgs.msg import CameraPlacement
from geometry_msgs.msg import Point, Vector3

# https://qiita.com/kitasenjudesign/items/e785e00736161ec238ae
def chokkou(r,theta,phi):
  x = r * math.sin(theta) * math.cos(phi);
  y = r * math.sin(theta) * math.sin(phi);
  z = r * math.cos(theta);
  return (x,y,z)

class Application(tk.Frame):

    def __init__(self, master = None):
        super().__init__(master)

        self.pub = rospy.Publisher("/rviz/camera_placement", CameraPlacement, queue_size = 1)

        self.x = self.create_scale("pi ", 0.01, math.pi, 0.01)
        self.y = self.create_scale("theta: ", -math.pi, math.pi, 0.01)
        self.z = self.create_scale("radius: ", 0, 20, 0.01)
        self.z.set(3)
        
        self.after(1, self.loop)

    def create_scale(self,name="", from_val=-1, to_val=1, resolution = 0.1,length=300,width=20, relese_func=None):

        label = tk.Label(self.master, text=name)
        label.pack()

        scale_var = tk.DoubleVar()

        scaleH = tk.Scale( self.master, 
                    orient=tk.HORIZONTAL,
                    variable = scale_var,
                    length = length,
                    width = width,             
                    sliderlength = 20,      
                    from_ = from_val,       
                    to = to_val,          
                    resolution = resolution,         
                    tickinterval = 50         
                    )

        scaleH.pack()

        return scale_var


    def loop(self):

        x = self.x.get()
        y = self.y.get()
        z = self.z.get()

        cx, cy, cz = chokkou(z, x, y)

        cp = CameraPlacement()

        p = Point(cx, cy, cz)
        cp.eye.point = p
        cp.eye.header.frame_id = "base_link"

        f = Point(0, 0, 0)
        cp.focus.point = f
        cp.focus.header.frame_id = "base_link"

        up = Vector3(0, 0, 1)
        cp.up.vector = up
        cp.up.header.frame_id = "base_link"

        cp.time_from_start = rospy.Duration(0.01)

        self.pub.publish(cp)

        print(x,y,z)

        self.after(1, self.loop)

        if rospy.is_shutdown():
          app.master.destroy()


rospy.init_node("slider_rviz_view", anonymous = True)

root = tk.Tk()
app = Application(master = root)
app.mainloop()
```

こんな感じ↓

![Peek 2022-08-20 21-17.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/ce3ff7d7-20cb-eb5a-b257-bbef2b2b3db2.gif)

# フォーカスポイントの移動

```python:add_move_fp.py
import tkinter as tk
import math

import rospy
from view_controller_msgs.msg import CameraPlacement
from geometry_msgs.msg import Point, Vector3
from geometry_msgs.msg import *

# https://qiita.com/kitasenjudesign/items/e785e00736161ec238ae
def chokkou(r,theta,phi):
  x = r * math.sin(theta) * math.cos(phi);
  y = r * math.sin(theta) * math.sin(phi);
  z = r * math.cos(theta);
  return (x,y,z)

class Application(tk.Frame):

    def __init__(self, master = None):
        super().__init__(master)

        self.pub = rospy.Publisher("/rviz/camera_placement", CameraPlacement, queue_size = 1)
        self.point_pub = rospy.Publisher("/rviz_forcus_point", PointStamped, queue_size = 1)

        self.x = self.create_scale("pi ", 0.01, math.pi, 0.01)
        self.y = self.create_scale("theta: ", -math.pi, math.pi, 0.01)
        self.z = self.create_scale("radius: ", 0, 20, 0.01)
        self.fx = self.create_scale("fx: ", -10, 10, 0.01)
        self.fy = self.create_scale("fy: ", -10, 10, 0.01)
        self.fz = self.create_scale("fz: ", -10, 10, 0.01)
        self.z.set(3)
        
        self.after(1, self.loop)

    def create_scale(self,name="", from_val=-1, to_val=1, resolution = 0.1,length=300,width=20, relese_func=None):

        label = tk.Label(self.master, text=name)
        label.pack()

        scale_var = tk.DoubleVar()

        scaleH = tk.Scale( self.master, 
                    orient=tk.HORIZONTAL,
                    variable = scale_var,
                    length = length,
                    width = width,             
                    sliderlength = 20,      
                    from_ = from_val,       
                    to = to_val,          
                    resolution = resolution,         
                    tickinterval = 50         
                    )

        scaleH.pack()

        return scale_var


    def loop(self):

        x = self.x.get()
        y = self.y.get()
        z = self.z.get()
        fx = self.fx.get()
        fy = self.fy.get()
        fz = self.fz.get()

        msg = PointStamped()
        msg.point.x = fx
        msg.point.y = fy
        msg.point.z = fz
        msg.header.frame_id = "base_link"
        self.point_pub.publish(msg)

        cx, cy, cz = chokkou(z, x, y)
        cx = fx + cx
        cy = fy + cy
        cz = fz + cz

        cp = CameraPlacement()

        p = Point(cx, cy, cz)
        cp.eye.point = p
        cp.eye.header.frame_id = "base_link"

        f = Point(fx, fy, fz)
        cp.focus.point = f
        cp.focus.header.frame_id = "base_link"

        up = Vector3(0, 0, 1)
        cp.up.vector = up
        cp.up.header.frame_id = "base_link"

        cp.time_from_start = rospy.Duration(0.01)

        self.pub.publish(cp)

        print(x,y,z)

        self.after(1, self.loop)

        if rospy.is_shutdown():
          app.master.destroy()


rospy.init_node("slider_rviz_view", anonymous = True)

root = tk.Tk()
app = Application(master = root)
app.mainloop()
```


![Peek 2022-11-27 13-59.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/451f5703-93b5-ac70-95fa-cdd045a01282.gif)