Go to the documentation of this file.00001
00002
00003
00004
00005 import rospy
00006 from sensor_msgs.msg import Image
00007 from geometry_msgs.msg import Pose
00008
00009 import copy
00010 import numpy as np
00011 import cv2
00012 from cv_bridge import CvBridge, CvBridgeError
00013 import tf
00014 import math
00015
00016 from rviz_textured_quads.msg import TexturedQuad, TexturedQuadArray
00017
00018
00019 def callback(data):
00020 bridge = CvBridge()
00021 cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
00022 img_msg1 = bridge.cv2_to_imgmsg(cv_image, "bgr8")
00023
00024 image_pub = rospy.Publisher("/semantic_targets", TexturedQuadArray, queue_size=10)
00025
00026 display_image = TexturedQuad()
00027 pose = Pose()
00028 pose.position.x = 0.0
00029 pose.position.y = 0.0
00030 pose.position.z = 0.0
00031
00032
00033 pose.orientation.x = 1.0
00034 pose.orientation.y = 0.0
00035 pose.orientation.z = 0.0
00036 pose.orientation.w = 0.0
00037
00038 display_image.image = img_msg1
00039 display_image.pose = pose
00040 display_image.width = 25
00041 display_image.height = 25
00042 display_image.border_color = [1., 0., 0., 0.5]
00043 display_image.border_size = 0.0
00044 display_image.caption = 'Directional Field'
00045 display_images = TexturedQuadArray()
00046 display_images = np.array([display_image])
00047
00048 image_pub.publish(display_images)
00049 rate = rospy.Rate(30)
00050 rate.sleep()
00051
00052 def listener():
00053
00054 rospy.init_node('listener', anonymous=True)
00055 image_sub = rospy.Subscriber("/tf_image",Image,callback)
00056 rospy.spin()
00057
00058
00059 if __name__ == '__main__':
00060
00061 try:
00062 listener()
00063 except rospy.ROSInterruptException:
00064 pass