Go to the documentation of this file.00001 from jsk_teleop_joy.camera_view import CameraView
00002
00003 import imp
00004 try:
00005 imp.find_module("view_controller_msgs")
00006 except:
00007 import roslib; roslib.load_manifest('jsk_teleop_joy')
00008
00009 from view_controller_msgs.msg import CameraPlacement
00010
00011 import tf
00012 import rospy
00013
00014 import numpy
00015 import math
00016
00017 import time
00018
00019 def signedSquare(val):
00020 if val > 0:
00021 sign = 1
00022 else:
00023 sign = -1
00024 return val * val * sign
00025
00026
00027 class RVizViewControllerManager():
00028 def __init__(self):
00029 self.camera_pub = rospy.Publisher('/rviz/camera_placement', CameraPlacement)
00030 self.pre_view = CameraView()
00031 self.follow_view = False
00032 self.counter = 0
00033 self.prev_time = rospy.Time.from_sec(time.time())
00034 def joyCB(self, status, history, pre_pose):
00035 self.counter = self.counter + 1
00036 if self.counter > 1024:
00037 self.counter = 0
00038 pre_view = self.pre_view
00039 view = CameraView()
00040 view.focus = numpy.copy(pre_view.focus)
00041 view.yaw = pre_view.yaw
00042 view.pitch = pre_view.pitch
00043 view.distance = pre_view.distance
00044 view_updated = False
00045 if status.R3:
00046 if not status.left_analog_y == 0.0:
00047 view.distance = view.distance - signedSquare(status.left_analog_y) * 0.05
00048 view_updated = True
00049
00050 if status.left:
00051 view_updated = True
00052 view_x = 1.0
00053 elif status.right:
00054 view_updated = True
00055 view_x = -1.0
00056 else:
00057 view_x = 0.0
00058 if status.up:
00059 view_updated = True
00060 view_y = 1.0
00061 elif status.down:
00062 view_updated = True
00063 view_y = -1.0
00064 else:
00065 view_y = 0.0
00066 focus_diff = numpy.dot(view.cameraOrientation(),
00067 numpy.array((view_x / 20.0 / view.distance,
00068 view_y / 20.0 / view.distance,
00069 0)))
00070 view.focus = view.focus + focus_diff
00071 else:
00072 if status.right_analog_x != 0.0:
00073 view_updated = True
00074 if status.right_analog_y != 0.0:
00075 view_updated = True
00076
00077 view.yaw = view.yaw - 0.05 * signedSquare(status.right_analog_x)
00078 view.pitch = view.pitch + 0.05 * signedSquare(status.right_analog_y)
00079
00080 if self.follow_view and self.support_follow_view:
00081 view_updated = True
00082 view.focus = numpy.array((pre_pose.pose.position.x,
00083 pre_pose.pose.position.y,
00084 pre_pose.pose.position.z))
00085
00086 q = numpy.array((pre_pose.pose.orientation.x,
00087 pre_pose.pose.orientation.y,
00088 pre_pose.pose.orientation.z,
00089 pre_pose.pose.orientation.w))
00090 mat = tf.transformations.quaternion_matrix(q)
00091 camera_local_pos = numpy.dot(mat, numpy.array((0, 0, 1, 1)))[:3]
00092 pitch = math.asin(camera_local_pos[2])
00093
00094 if camera_local_pos[1] < 0:
00095 pitch = math.pi - pitch
00096 if math.fabs(math.cos(pitch)) < 0.01:
00097 yaw = 0.0
00098 else:
00099 cos_value = camera_local_pos[0] / math.cos(pitch)
00100 if cos_value > 1.0:
00101 cos_value = 1.0
00102 elif cos_value < -1.0:
00103 cos_value = -1.0
00104 yaw = math.acos(cos_value)
00105 view.pitch = pitch
00106 view.yaw = yaw
00107 z_up = numpy.dot(mat, numpy.array((1, 0, 0, 1)))
00108 view.z_up = z_up[:3]
00109 now = rospy.Time.from_sec(time.time())
00110 placement = view.cameraPlacement()
00111 placement.time_from_start = now - self.prev_time
00112 if (now - self.prev_time).to_sec() > 1 / 10.0:
00113 self.camera_pub.publish(placement)
00114 self.prev_time = now
00115 self.pre_view = view
00116
00117
00118 RVizViewControllerManagerSingleton = RVizViewControllerManager()
00119
00120