jsk_perception: fisheye_ray.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import cv2
00003 
00004 import rospy
00005 import cv_bridge
00006 from sensor_msgs.msg import Image, CameraInfo
00007 from geometry_msgs.msg import Vector3, PoseStamped, PointStamped
00008 import math
00009 import tf
00010 import numpy as np
00011 rospy.init_node("click_to_pose")
00012 
00013 latest_camera_info=None
00014 pub = rospy.Publisher("~output", PoseStamped, queue_size=1)
00015 pub_p = rospy.Publisher("~output_point", PointStamped, queue_size=1)
00016 
00017 def camera_info_cb(msg):
00018     global latest_camera_info
00019     latest_camera_info = msg;
00020 
00021 def image_cb(msg):
00022     global latest_image
00023     latest_image = msg;
00024 
00025 def cloud_cb(msg):
00026     global latest_camera_info, latest_image, frame_id
00027     # if latest_camera_info:
00028     if latest_image:
00029     #     r = math.sqrt((msg.point.x - latest_camera_info.width/2)*(msg.point.x - latest_camera_info.width/2)+ (msg.point.y - latest_camera_info.height/2.0) * (msg.point.y - latest_camera_info.height/2.0))
00030         r = math.sqrt((msg.point.x - latest_image.width/2)*(msg.point.x - latest_image.width/2)+ (msg.point.y - latest_image.height/2.0) * (msg.point.y - latest_image.height/2.0))
00031         phi=r/341.0
00032         # x = -(msg.point.y - latest_camera_info.height/2.0) / r * math.sin(phi)
00033         # y = (msg.point.x - latest_camera_info.width/2.0) / r * math.sin(phi)
00034         x = -(msg.point.y - latest_image.height/2.0) / r * math.sin(phi)
00035         y = (msg.point.x - latest_image.width/2.0) / r * math.sin(phi)
00036         z = 1.0 * math.cos(phi)
00037         pose = PoseStamped()
00038         pose.pose.position.x = 0
00039         pose.pose.position.y = 0
00040         pose.pose.position.z = 0
00041 
00042         first_vec = [x, -y,-z]
00043         second_vec = [1,0,0]
00044         dot = np.dot(first_vec, second_vec)/(tf.transformations.vector_norm(first_vec)*tf.transformations.vector_norm(second_vec))
00045         c = np.arccos(dot)
00046 
00047         M = tf.transformations.rotation_matrix(c,np.cross(first_vec, second_vec))
00048         quat = tf.transformations.quaternion_from_matrix(M)
00049         pose.pose.orientation.x = quat[0]
00050         pose.pose.orientation.y = quat[1]
00051         pose.pose.orientation.z = quat[2]
00052         pose.pose.orientation.w = quat[3]
00053 
00054         pose.header.frame_id=frame_id
00055         pub.publish(pose)
00056 
00057         point = PointStamped()
00058         point.header.stamp = rospy.Time.now()
00059         point.header.frame_id= frame_id
00060         point.point.x = x
00061         point.point.y = y
00062         point.point.z = z
00063         pub_p.publish(point)
00064 
00065 if __name__ == "__main__":
00066     frame_id = rospy.get_param("~frame_id", "fisheye")
00067     rospy.Subscriber("clicked_point", PointStamped, cloud_cb)
00068     rospy.Subscriber("camera_info", CameraInfo, camera_info_cb)
00069     rospy.Subscriber("image", Image, image_cb)
00070     rospy.spin()