5 imp.find_module(
"view_controller_msgs")
7 import roslib; roslib.load_manifest(
'jsk_teleop_joy')
9 from view_controller_msgs.msg
import CameraPlacement
24 return val * val * sign
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) 35 self.
camera_pub = rospy.Publisher(
'/rviz/camera_placement', CameraPlacement, queue_size = 1)
36 self.
camera_sub = rospy.Subscriber(
'/rviz/current_camera_placement', CameraPlacement,
43 self.
pre_view = CameraView.createFromCameraPlacement(msg)
44 def joyCB(self, status, history, pre_pose):
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
63 view.focus = numpy.array((pre_pose.pose.position.x,
64 pre_pose.pose.position.y,
65 pre_pose.pose.position.z))
69 if not status.left_analog_y == 0.0:
70 view.distance = view.distance -
signedSquare(status.left_analog_y) * 0.05
91 if not status.left_analog_x == 0.0:
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
100 if status.right_analog_x != 0.0:
102 if status.right_analog_y != 0.0:
105 view.yaw = view.yaw - 0.05 *
signedSquare(status.right_analog_x)
106 view.pitch = view.pitch + 0.05 *
signedSquare(status.right_analog_y)
108 if (int(math.floor((2 * math.fabs(view.pitch))/math.pi)) + 1) % 4 > 1:
114 view.focus = numpy.array((pre_pose.pose.position.x,
115 pre_pose.pose.position.y,
116 pre_pose.pose.position.z))
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])
126 if camera_local_pos[1] < 0:
127 pitch = math.pi - pitch
128 if math.fabs(math.cos(pitch)) < 0.01:
131 cos_value = camera_local_pos[0] / math.cos(pitch)
134 elif cos_value < -1.0:
136 yaw = math.acos(cos_value)
139 z_up = numpy.dot(mat, numpy.array((1, 0, 0, 1)))
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:
def joyCB(self, status, history, pre_pose)
def publishView(self, view)