rviz_view_controller_singleton.py
Go to the documentation of this file.
1 from jsk_teleop_joy.camera_view import CameraView
2 
3 import imp
4 try:
5  imp.find_module("view_controller_msgs")
6 except:
7  import roslib; roslib.load_manifest('jsk_teleop_joy')
8 
9 from view_controller_msgs.msg import CameraPlacement
10 
11 import tf
12 import rospy
13 
14 import numpy
15 import math
16 
17 import time
18 
19 def signedSquare(val):
20  if val > 0:
21  sign = 1
22  else:
23  sign = -1
24  return val * val * sign
25 
26 
28  '''
29 Description:
30  This class is used as a singleton instance named RVizViewControllerManagerSingleton.
31  Plugin class using this class is RVizViewController.
32  Use TabletViewController(jsk_rviz_plugin) in rviz (selected at 'Views' Panel)
33  '''
34  def __init__(self):
35  self.camera_pub = rospy.Publisher('/rviz/camera_placement', CameraPlacement, queue_size = 1)
36  self.camera_sub = rospy.Subscriber('/rviz/current_camera_placement', CameraPlacement,
37  self.cameraCB, queue_size = 1)
39  self.follow_view = False
40  self.counter = 0
41  self.prev_time = rospy.Time.from_sec(time.time())
42  def cameraCB(self, msg):
43  self.pre_view = CameraView.createFromCameraPlacement(msg)
44  def joyCB(self, status, history, pre_pose):
45  self.counter = self.counter + 1
46  if self.counter > 1024:
47  self.counter = 0
48  pre_view = self.pre_view
49  view = CameraView()
50  view.focus = numpy.copy(pre_view.focus)
51  view.yaw = pre_view.yaw
52  view.pitch = pre_view.pitch
53  view.distance = pre_view.distance
54  view_updated = False
55  if status.R3:
56  if status.L3:
57 
58  v = CameraView()
59  self.publishView(v)
60  return
61  if status.L1:
62 
63  view.focus = numpy.array((pre_pose.pose.position.x,
64  pre_pose.pose.position.y,
65  pre_pose.pose.position.z))
66  self.publishView(view)
67  return
68 
69  if not status.left_analog_y == 0.0:
70  view.distance = view.distance - signedSquare(status.left_analog_y) * 0.05
71  view_updated = True
72  # calc camera orietation(x)
73  if status.left:
74  view_updated = True
75  view_x = 1.0
76  elif status.right:
77  view_updated = True
78  view_x = -1.0
79  else:
80  view_x = 0.0
81  # calc camera orietation(y)
82  if status.up:
83  view_updated = True
84  view_y = 1.0
85  elif status.down:
86  view_updated = True
87  view_y = -1.0
88  else:
89  view_y = 0.0
90 
91  if not status.left_analog_x == 0.0:
92  view_updated = True
93 
94  focus_diff = numpy.dot(view.cameraOrientation(),
95  numpy.array((view_x / 7.0 / view.distance,
96  view_y / 7.0 / view.distance,
97  - status.left_analog_x / 7.0 / view.distance)))
98  view.focus = view.focus + focus_diff
99  else:
100  if status.right_analog_x != 0.0:
101  view_updated = True
102  if status.right_analog_y != 0.0:
103  view_updated = True
104 
105  view.yaw = view.yaw - 0.05 * signedSquare(status.right_analog_x)
106  view.pitch = view.pitch + 0.05 * signedSquare(status.right_analog_y)
107 
108  if (int(math.floor((2 * math.fabs(view.pitch))/math.pi)) + 1) % 4 > 1:
109  view.z_up[2] = -1
110 
111 
112  if self.follow_view and self.support_follow_view:
113  view_updated = True
114  view.focus = numpy.array((pre_pose.pose.position.x,
115  pre_pose.pose.position.y,
116  pre_pose.pose.position.z))
117  #view.yaw = math.pi
118  q = numpy.array((pre_pose.pose.orientation.x,
119  pre_pose.pose.orientation.y,
120  pre_pose.pose.orientation.z,
121  pre_pose.pose.orientation.w))
122  mat = tf.transformations.quaternion_matrix(q)
123  camera_local_pos = numpy.dot(mat, numpy.array((0, 0, 1, 1)))[:3]
124  pitch = math.asin(camera_local_pos[2])
125  # calc pitch quadrant
126  if camera_local_pos[1] < 0:
127  pitch = math.pi - pitch
128  if math.fabs(math.cos(pitch)) < 0.01:
129  yaw = 0.0
130  else:
131  cos_value = camera_local_pos[0] / math.cos(pitch)
132  if cos_value > 1.0:
133  cos_value = 1.0
134  elif cos_value < -1.0:
135  cos_value = -1.0
136  yaw = math.acos(cos_value)
137  view.pitch = pitch
138  view.yaw = yaw
139  z_up = numpy.dot(mat, numpy.array((1, 0, 0, 1)))
140  view.z_up = z_up[:3]
141  if view_updated:
142  self.publishView(view)
143 
144  def publishView(self, view):
145  now = rospy.Time.from_sec(time.time())
146  placement = view.cameraPlacement()
147  placement.time_from_start = now - self.prev_time
148  if (now - self.prev_time).to_sec() > 1 / 10.0:
149  self.camera_pub.publish(placement)
150  self.prev_time = now
151  self.pre_view = view
152 
153 RVizViewControllerManagerSingleton = RVizViewControllerManager()


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:37