pose_visualizer.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('checkerboard_detector2')
import rospy
import sys
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Vector3, Point, Pose, PoseArray
from std_msgs.msg import ColorRGBA
from sensor_msgs.msg import ChannelFloat32
import logging
import getopt
import colorsys
import numpy
from transform_datatypes import * 

def usage(progname):
  print __doc__ % vars()

class trackVisualizer:
  def __init__(self):
    self.pub = rospy.Publisher('visualization_marker', Marker)
    self.pub_array = rospy.Publisher('visualization_marker_array', MarkerArray)
    rospy.Subscriber("/camera/pose_array", PoseArray, self.callback)
    self.show_names = rospy.get_param('~show_names', True)
  
  def callback(self, pose_array):
    #rospy.loginfo("received pose array of length %d" % len(pose_array.poses))
    marker_array = MarkerArray()
    
    self.render_axis(pose_array, marker_array)

    #rospy.loginfo("publishing MarkerArray, containing %d markers",
    #  len(marker_array.markers))
    self.pub_array.publish(marker_array)
    
  def render_axis(self, pose_array, marker_array):
    all_detected = True
    for p in pose_array.poses:
      r = [p.orientation.x,p.orientation.y,p.orientation.z,p.orientation.w]
      if( numpy.dot(r,r) == 0 ):
        all_detected = False
      
    for i in range(len(pose_array.poses)):
      for axis in range(3):
          marker = Marker()
          marker.header.stamp = pose_array.header.stamp
          marker.header.frame_id = pose_array.header.frame_id
          marker.ns = "pose_axis_%d" % (i)
          marker.scale = Vector3(0.003, 0.003, 0.003)
          marker.id = (i*3 + axis)*3
          marker.action = Marker.ADD
          if all_detected:
            marker.color = self.generate_color_axis( axis / 3.0 )
          else:
            marker.color = self.generate_color_axis( 0.00 )
          marker.type = Marker.LINE_STRIP
          marker.pose = pose_array.poses[i]
          if axis == 0:
            marker.points.append(Point(0, 0, 0))
            marker.points.append(Point(0.05, 0, 0))
          elif axis == 1:
            marker.points.append(Point(0, 0, 0))
            marker.points.append(Point(0, 0.05, 0))
          elif axis == 2:
            marker.points.append(Point(0, 0, 0))
            marker.points.append(Point(0, 0, 0.05))
          marker_array.markers.append(marker)
          
    if self.show_names:
      for i in range(len(pose_array.poses)):
        marker = Marker()
        marker.header.stamp = pose_array.header.stamp
        marker.header.frame_id = pose_array.header.frame_id
        marker.ns = "pose_name_%d" % (i)
        marker.scale = Vector3(0.05, 0.05, 0.05)
        marker.id = i
        marker.action = Marker.ADD
        if all_detected:
          marker.color = ColorRGBA(1.0,1.0,1.0, 1.0)
        else:
          marker.color = ColorRGBA(1.0,0.0,0.0, 1.0)
        marker.type = Marker.TEXT_VIEW_FACING
        marker.pose = mat44_to_pose((numpy.dot(pose_to_mat44(pose_array.poses[i]),
                                               tf.transformations.translation_matrix([0.0,0.0,0]))))
        marker.text = "x%d"%(i+1)
        marker_array.markers.append(marker)
          
        

  def generate_color_axis(self, h=0,s=1,v=1):
    rgb = colorsys.hsv_to_rgb(h, s, v)
    return ColorRGBA(rgb[0], rgb[1], rgb[2], 1.0)


def main():
  try:
    rospy.init_node('pose_array_visualizer')
    trackVisualizer()
    rospy.spin()
  except rospy.ROSInterruptException: pass
    
if __name__ == '__main__':
  main()