Go to the documentation of this file.00001 import roslib
00002 roslib.load_manifest('rospy')
00003 roslib.load_manifest('visualization_msgs')
00004 import rospy
00005 from visualization_msgs.msg import Marker
00006 import numpy as np
00007
00008 class SceneDraw:
00009 def __init__(self, topic='contact_visualization', node='contact_sim', frame='/world_frame'):
00010 self.pub = rospy.Publisher(topic+'_marker', Marker)
00011 self.frame = frame
00012 self.Marker = Marker
00013
00014 def pub_body(self, pos, quat, scale, color, num, shape, text = ''):
00015 marker = Marker()
00016 marker.header.frame_id = self.frame
00017 marker.header.stamp = rospy.Time.now()
00018
00019 marker.ns = "basic_shapes"
00020 marker.id = num
00021
00022 marker.type = shape
00023 marker.action = Marker.ADD
00024
00025 marker.pose.position.x = pos[0]
00026 marker.pose.position.y = pos[1]
00027 marker.pose.position.z = pos[2]
00028 marker.pose.orientation.x = quat[0]
00029 marker.pose.orientation.y = quat[1]
00030 marker.pose.orientation.z = quat[2]
00031 marker.pose.orientation.w = quat[3]
00032 marker.scale.x = scale[0]
00033 marker.scale.y = scale[1]
00034 marker.scale.z = scale[2]
00035 marker.color.r = color[0]
00036 marker.color.g = color[1]
00037 marker.color.b = color[2]
00038 marker.color.a = color[3]
00039 marker.lifetime = rospy.Duration()
00040 marker.text = text
00041 self.pub.publish(marker)
00042
00043 def get_rot_mat(self, rot):
00044
00045 rot_mat = np.matrix([[rot[0], rot[4], rot[8]],[rot[1], rot[5], rot[9]], [rot[2], rot[6], rot[10]]])
00046 return rot_mat
00047
00048
00049
00050