Go to the documentation of this file.00001
00002
00003 import rospy
00004 from jsk_topic_tools import ConnectionBasedTransport
00005 from jsk_recognition_msgs.msg import Rect, RectArray
00006 from image_view2.msg import ImageMarker2
00007 import jsk_recognition_utils
00008 from std_msgs.msg import ColorRGBA
00009 from geometry_msgs.msg import Point
00010 class RectArrayToImageMarker(ConnectionBasedTransport):
00011 def __init__(self):
00012 super(RectArrayToImageMarker, self).__init__()
00013 self.pub = self.advertise("~output", ImageMarker2, queue_size=1)
00014
00015 def subscribe(self):
00016 self.sub = rospy.Subscriber("~input", RectArray, self.convert)
00017
00018 def unsubscribe(self):
00019 self.sub.unregister()
00020
00021 def convert(self, msg):
00022 marker = ImageMarker2()
00023 marker.header = msg.header
00024 marker.type = ImageMarker2.LINE_LIST
00025 n_colors = min(len(msg.rects), 256)
00026 cmap = jsk_recognition_utils.color.labelcolormap(n_colors)
00027 for rect, rect_i in zip(msg.rects, range(n_colors)):
00028 points = [(rect.x, rect.y),
00029 (rect.x, rect.y + rect.height),
00030 (rect.x + rect.width, rect.y + rect.height),
00031 (rect.x + rect.width, rect.y)]
00032 color = cmap[rect_i]
00033 color_msg = ColorRGBA(r=color[0], g=color[1], b=color[2])
00034 for i, j in ((0, 1), (1, 2), (2, 3), (3, 0)):
00035 marker.points.append(Point(x=points[i][0], y=points[i][1]))
00036 marker.points.append(Point(x=points[j][0], y=points[j][1]))
00037 marker.outline_colors.append(color_msg)
00038 marker.outline_colors.append(color_msg)
00039 self.pub.publish(marker)
00040
00041
00042 if __name__ == '__main__':
00043 rospy.init_node('rect_array_to_image_marker')
00044 rect_array_to_image_marker = RectArrayToImageMarker()
00045 rospy.spin()