visualize_planes.py

Go to the documentation of this file.

#!/usr/bin/env python
'''
visualize_planes.py
Mac Mason <mac@cs.duke.edu>

Given the planes computed in hand_planes.py, generate some helpful
visualizations.
'''
import sys
import random
import shapely.geometry as geo
import roslib; roslib.load_manifest('semanticmodel')
import rospy
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
import visualization_msgs
import hand_planes

def make_a_color():
  return (random.random(), random.random(), random.random())

if __name__ == "__main__":
  rospy.init_node('visualize_planes')
  if len(sys.argv) != 2:
    print "Usage: visualize_planes <id>"
    sys.exit(1)

  index = int(sys.argv[1])
  pub = rospy.Publisher('/visualization_marker_array2', MarkerArray)

  v = hand_planes.meters[index]

  ma = MarkerArray()
  for idx, k in enumerate(v):
    m = Marker()
    c = make_a_color()
    m.header.frame_id = "/map"
    m.header.stamp = rospy.Time.now()
    m.ns = "planes"
    m.id = idx
    m.type = visualization_msgs.msg.Marker.LINE_LIST
    m.action = visualization_msgs.msg.Marker.ADD
    m.scale.x = 0.05
    m.color.r = c[0]
    m.color.g = c[1]
    m.color.b = c[2]
    m.color.a = 1.0
    for i in range(4):
      m.points.append(Point())
      m.points[-1].x = v[k][i][0]
      m.points[-1].y = v[k][i][1]
      m.points[-1].z = v[k][i][2]
      m.points.append(Point())
      m.points[-1].x = v[k][(i + 1) % 4][0]
      m.points[-1].y = v[k][(i + 1) % 4][1]
      m.points[-1].z = v[k][(i + 1) % 4][2]
    poly = geo.geo.Polygon([(k.x, k.y) for k in m.points])
    if poly.area < 0.25:
      continue
    ma.markers.append(m)

  while not rospy.is_shutdown():
    pub.publish(ma)
    rospy.sleep(1.0)