00001
00002
00003 import numpy as np
00004 import copy
00005
00006 import roslib
00007 roslib.load_manifest('hrl_ellipsoidal_control')
00008
00009 import rospy
00010 from visualization_msgs.msg import Marker, MarkerArray
00011 from std_msgs.msg import ColorRGBA
00012 from geometry_msgs.msg import PoseStamped, PoseArray, Vector3
00013
00014 import hrl_geom.transformations as trans
00015 from hrl_ellipsoidal_control.msg import EllipsoidParams
00016 from hrl_geom.pose_converter import PoseConv
00017 from hrl_ellipsoidal_control.ellipsoid_space import EllipsoidSpace
00018
00019 eye_scale = Vector3(0.02, 0.01, 0.010)
00020 l_eye_loc = [(3.5 * np.pi/8, -0.9 * np.pi/8, 1.20), ( np.pi/2, np.pi/2, 0)]
00021 r_eye_loc = [(3.5 * np.pi/8, 0.9 * np.pi/8, 1.20), ( np.pi/2, np.pi/2, 0)]
00022
00023 mouth_scale = Vector3(0.05, 0.01, 0.010)
00024 mouth_loc = [(4.7 * np.pi/8, 0 * np.pi/8, 1.25), ( np.pi/2, np.pi/2, 0)]
00025
00026 ear_scale = Vector3(0.06, 0.03, 0.030)
00027 l_ear_loc = [( 4 * np.pi/8, -3.9 * np.pi/8, 1.10), ( 0, np.pi/2, 0)]
00028 r_ear_loc = [( 4 * np.pi/8, 3.9 * np.pi/8, 1.10), ( 0, np.pi/2, 0)]
00029
00030 class HeadMarkers(object):
00031 def __init__(self):
00032 self.ell_space = EllipsoidSpace(1)
00033 self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
00034 self.found_params = False
00035
00036 def read_params(self, e_params):
00037 self.ell_space.load_ell_params(e_params.E, e_params.is_oblate, e_params.height)
00038 if not self.found_params:
00039 rospy.loginfo("[head_markers] Found params from /ellipsoid_params")
00040 self.found_params = True
00041
00042 def create_eye_marker(self, pose, m_id, color=ColorRGBA(1., 1., 1., 1.)):
00043 m = Marker()
00044
00045 m.header.frame_id = "/ellipse_frame"
00046 m.header.stamp = rospy.Time.now()
00047 m.ns = "head_markers"
00048 m.id = m_id
00049 m.type = Marker.CYLINDER
00050 m.action = Marker.ADD
00051 m.scale = eye_scale
00052 m.color = color
00053 m.pose = PoseConv.to_pose_msg(pose)
00054 return m
00055
00056 def create_mouth_marker(self, pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
00057 m = Marker()
00058
00059 m.header.frame_id = "/ellipse_frame"
00060 m.header.stamp = rospy.Time.now()
00061 m.ns = "head_markers"
00062 m.id = m_id
00063 m.type = Marker.CYLINDER
00064 m.action = Marker.ADD
00065 m.scale = mouth_scale
00066 m.color = color
00067 m.pose = PoseConv.to_pose_msg(pose)
00068 return m
00069
00070 def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
00071 m = Marker()
00072
00073 m.header.frame_id = "/ellipse_frame"
00074 m.header.stamp = rospy.Time.now()
00075 m.ns = "head_markers"
00076 m.id = m_id
00077 m.type = Marker.CYLINDER
00078 m.action = Marker.ADD
00079 m.scale = ear_scale
00080 m.color = color
00081 m.pose = PoseConv.to_pose_msg(pose)
00082 return m
00083
00084 def get_head(self):
00085 if not self.found_params:
00086 return
00087 head_array = MarkerArray()
00088 head_array.markers.append(
00089 self.create_eye_marker(self.get_head_pose(l_eye_loc), 0))
00090 head_array.markers.append(
00091 self.create_eye_marker(self.get_head_pose(r_eye_loc), 1))
00092 head_array.markers.append(
00093 self.create_mouth_marker(self.get_head_pose(mouth_loc), 2))
00094 head_array.markers.append(
00095 self.create_ear_marker(self.get_head_pose(l_ear_loc), 3))
00096 head_array.markers.append(
00097 self.create_ear_marker(self.get_head_pose(r_ear_loc), 4))
00098 return head_array
00099
00100 def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
00101 lat, lon, height = ell_coords_rot[0]
00102 roll, pitch, yaw = ell_coords_rot[1]
00103 pos, rot = PoseConv.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
00104 rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
00105 return pos, rot
00106
00107 def main():
00108 rospy.init_node("head_markers")
00109 hm = HeadMarkers()
00110 pub_head = rospy.Publisher("visualization_markers_array", MarkerArray)
00111 while not rospy.is_shutdown():
00112 head = hm.get_head()
00113 pub_head.publish(head)
00114 rospy.sleep(0.1)
00115
00116
00117 if __name__ == "__main__":
00118 main()