spatial_service.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib;
00003 roslib.load_manifest('srs_knowledge')
00004 import sys
00005 import rospy
00006 
00007 from srs_knowledge.srv import *
00008 from srs_knowledge.msg import *
00009 
00010 import numpy as np
00011 
00012 def transform_2d_point(theta, x, y):
00013     nx, ny = x, y
00014     R = np.array([np.cos(theta), -np.sin(theta), np.sin(theta), np.cos(theta)])
00015     R = R.reshape(2,2)
00016     pos = np.array([x,y])
00017     pos = pos.reshape(2,1)
00018     new_pos = R.dot(pos)
00019     return new_pos
00020 
00021 if __name__=='__main__':
00022     print 'service ---- for spatial relation calculation'
00023     print transform_2d_point(np.pi, -1, 1)
00024     


srs_knowledge
Author(s): Ze Ji
autogenerated on Mon Oct 6 2014 08:27:32