00001
00002 '''
00003 visualize_planes.py
00004 Mac Mason <mac@cs.duke.edu>
00005
00006 Given the planes computed in hand_planes.py, generate some helpful
00007 visualizations.
00008 '''
00009 import sys
00010 import random
00011 import shapely.geometry as geo
00012 import roslib; roslib.load_manifest('semanticmodel')
00013 import rospy
00014 from visualization_msgs.msg import Marker, MarkerArray
00015 from geometry_msgs.msg import Point
00016 import visualization_msgs
00017 import hand_planes
00018
00019 def make_a_color():
00020 return (random.random(), random.random(), random.random())
00021
00022 if __name__ == "__main__":
00023 rospy.init_node('visualize_planes')
00024 if len(sys.argv) != 2:
00025 print "Usage: visualize_planes <id>"
00026 sys.exit(1)
00027
00028 index = int(sys.argv[1])
00029 pub = rospy.Publisher('/visualization_marker_array2', MarkerArray)
00030
00031 v = hand_planes.meters[index]
00032
00033 ma = MarkerArray()
00034 for idx, k in enumerate(v):
00035 m = Marker()
00036 c = make_a_color()
00037 m.header.frame_id = "/map"
00038 m.header.stamp = rospy.Time.now()
00039 m.ns = "planes"
00040 m.id = idx
00041 m.type = visualization_msgs.msg.Marker.LINE_LIST
00042 m.action = visualization_msgs.msg.Marker.ADD
00043 m.scale.x = 0.05
00044 m.color.r = c[0]
00045 m.color.g = c[1]
00046 m.color.b = c[2]
00047 m.color.a = 1.0
00048 for i in range(4):
00049 m.points.append(Point())
00050 m.points[-1].x = v[k][i][0]
00051 m.points[-1].y = v[k][i][1]
00052 m.points[-1].z = v[k][i][2]
00053 m.points.append(Point())
00054 m.points[-1].x = v[k][(i + 1) % 4][0]
00055 m.points[-1].y = v[k][(i + 1) % 4][1]
00056 m.points[-1].z = v[k][(i + 1) % 4][2]
00057 poly = geo.geo.Polygon([(k.x, k.y) for k in m.points])
00058 if poly.area < 0.25:
00059 continue
00060 ma.markers.append(m)
00061
00062 while not rospy.is_shutdown():
00063 pub.publish(ma)
00064 rospy.sleep(1.0)
