jsk_teleop_joy: camera_view.py Source File

Go to the documentation of this file.
00001 # camera_view.py
00002 import math
00003 import numpy
00004 import tf
00005 import rospy
00006 import imp
00007 try:
00008   imp.find_module("view_controller_msgs")
00009 except:
00010   import roslib; roslib.load_manifest('jsk_teleop_joy')
00011 
00012 from view_controller_msgs.msg import CameraPlacement
00013 
00014 class CameraView():
00015   def __init__(self):
00016     self.yaw = 0.0
00017     self.pitch = 0.0
00018     #self.roll = 0.0
00019     self.distance = 2.0
00020     self.focus = numpy.array((0, 0, 0))
00021     self.z_up = numpy.array((0, 0, 1))
00022   def viewPoint(self):
00023     p = numpy.array((self.distance * math.cos(self.yaw) * math.cos(self.pitch) + self.focus[0],
00024                      self.distance * math.sin(self.yaw) * math.cos(self.pitch) + self.focus[1],
00025                      self.distance *                      math.sin(self.pitch) + self.focus[2]))
00026     return p
00027   def cameraOrientation(self):
00028     OE = self.viewPoint()
00029     f = self.focus - OE # z
00030     f = f / tf.transformations.vector_norm(f)
00031     u = numpy.array((0, 0, 1))            #not aligned y
00032     r = numpy.cross(u, f) # x
00033     r = r / tf.transformations.vector_norm(r)
00034     uy = numpy.cross(f, r)
00035     uy = uy / tf.transformations.vector_norm(uy)
00036     m = tf.transformations.identity_matrix()[:3, :3]   #rotation matrix
00037     m[0, 0] = r[0]
00038     m[1, 0] = r[1]
00039     m[2, 0] = r[2]
00040     m[0, 1] = uy[0]
00041     m[1, 1] = uy[1]
00042     m[2, 1] = uy[2]
00043     m[0, 2] = f[0]
00044     m[1, 2] = f[1]
00045     m[2, 2] = f[2]
00046     return m
00047   def cameraPlacement(self):
00048     #TIME = 0.05
00049     #TIME = 1.0 / 40 * 2.0
00050     TIME = 1.0 / 30
00051     view_point = self.viewPoint()
00052     placement = CameraPlacement()
00053     placement.interpolation_mode = CameraPlacement.LINEAR
00054     #placement.interpolation_mode = CameraPlacement.SPHERICAL
00055     placement.time_from_start = rospy.Duration(TIME)
00056     # placement.eye.header.stamp = rospy.Time(0.0)
00057     placement.eye.header.stamp = rospy.Time.now()
00058     placement.eye.point.x = view_point[0]
00059     placement.eye.point.y = view_point[1]
00060     placement.eye.point.z = view_point[2]
00061     placement.focus.header.stamp = rospy.Time.now()
00062     placement.focus.point.x = self.focus[0]
00063     placement.focus.point.y = self.focus[1]
00064     placement.focus.point.z = self.focus[2]
00065     placement.up.header.stamp = rospy.Time.now()
00066     placement.up.vector.x = self.z_up[0]
00067     placement.up.vector.y = self.z_up[1]
00068     placement.up.vector.z = self.z_up[2]
00069     placement.mouse_interaction_mode = CameraPlacement.ORBIT
00070     return placement