hrl_common_code_darpa_m3: draw_scene.py Source File

Go to the documentation of this file.
00001 
00002 #
00003 # common visualization helper code for software simulation.
00004 #
00005 
00006 import roslib
00007 import math
00008 roslib.load_manifest('hrl_common_code_darpa_m3')
00009 import rospy
00010 from visualization_msgs.msg import Marker
00011 from geometry_msgs.msg import Point
00012 import numpy as np
00013 
00014 class SceneDraw:
00015     def __init__(self, topic='sim/viz/bodies', frame='/world'):
00016         self.pub = rospy.Publisher(topic, Marker)
00017         self.frame = frame
00018         self.Marker = Marker
00019 
00020     def pub_body(self, pos, quat, scale, color, num, shape, text = ''):
00021         marker = Marker()
00022         marker.header.frame_id = self.frame
00023         marker.header.stamp = rospy.Time.now()
00024 
00025         marker.ns = "basic_shapes"
00026         marker.id = num
00027 
00028         marker.type = shape
00029         marker.action = Marker.ADD
00030 
00031         marker.pose.position.x = pos[0]
00032         marker.pose.position.y = pos[1]
00033         marker.pose.position.z = pos[2]
00034         marker.pose.orientation.x = quat[0]
00035         marker.pose.orientation.y = quat[1]
00036         marker.pose.orientation.z = quat[2]
00037         marker.pose.orientation.w = quat[3]
00038         marker.scale.x = scale[0]
00039         marker.scale.y = scale[1]
00040         marker.scale.z = scale[2]
00041         marker.color.r = color[0]
00042         marker.color.g = color[1]
00043         marker.color.b = color[2]
00044         marker.color.a = color[3]
00045         marker.lifetime = rospy.Duration()
00046         marker.text = text
00047         self.pub.publish(marker)
00048 
00049     def pub_arrow(self, pos1, pos2, color, mag_force):
00050         marker = Marker()
00051         marker.header.frame_id = self.frame
00052         marker.header.stamp = rospy.Time.now()
00053 
00054         marker.ns = "basic_shapes"
00055         marker.id = 99999
00056 
00057         marker.type = Marker.ARROW
00058         marker.action = Marker.ADD
00059 
00060         pt1 = Point()
00061         pt2 = Point()
00062         pt1.x = pos1[0,0]
00063         pt1.y = pos1[1,0]
00064         pt1.z = pos1[2,0]
00065         pt2.x = pos2[0,0]
00066         pt2.y = pos2[1,0]
00067         pt2.z = pos2[2,0]
00068         marker.points.append(pt1)
00069         marker.points.append(pt2)
00070 
00071         marker.scale.x = 0.05
00072         marker.scale.y = 0.1
00073         #marker.scale.z = scale[2]
00074         marker.color.r = color[0]
00075         marker.color.g = color[1]
00076         marker.color.b = color[2]
00077         marker.color.a = color[3]
00078         marker.lifetime = rospy.Duration()
00079         marker.text = mag_force
00080         self.pub.publish(marker)
00081 
00082 
00083     def get_rot_mat(self, rot):
00084 #        rot_mat = np.matrix([[rot[0], rot[3], rot[6]],[rot[1], rot[4], rot[7]], [rot[2], rot[5], rot[8]]])
00085         rot_mat = np.matrix([[rot[0], rot[4], rot[8]],[rot[1], rot[5], rot[9]], [rot[2], rot[6], rot[10]]])
00086         return rot_mat
00087 
00088     def quat_from_axis_angle(self, axis, angle=math.pi/2.0):
00089         np_axis = np.array(axis)
00090         unit_np_ax = np_axis / np.linalg.norm(np_axis)
00091         if angle != 0:
00092             quat = [math.cos(angle/2.0), math.sin(angle/2.0)*unit_np_ax[0], math.sin(angle/2.0)*unit_np_ax[1],
00093                     math.sin(angle/2.0)*unit_np_ax[2]]
00094         else:
00095             print "angle cannot be zero in quat_from_axis_angle, it isn't defined"
00096         return quat
00097 
00098