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