def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.head_pose_srv = rospy.Service("/get_head_pose", GetHeadPose,
self.get_head_pose_srv)
self.lock_ell = False
self.found_params = False
def __init__(self, arm):
self.time_step = 1. / 20.
self.gripper_rot = np.pi
self.arm = arm
self.ell_traj_behavior = EPC("ellipsoid_traj")
self.ell_space = EllipsoidSpace(1)
self.tf_list = tf.TransformListener()
self.found_params = False
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.start_pub = rospy.Publisher("/start_pose", PoseStamped)
self.end_pub = rospy.Publisher("/end_pose", PoseStamped)
self.ctrl_switcher = ControllerSwitcher()
self.ell_cmd_lock = Lock()
self.params_lock = Lock()
self.ell_ep = None
self.action_preempted = False
self.ell_move_act = actionlib.SimpleActionServer(
"/ellipsoid_move", EllipsoidMoveAction, self.command_move_exec,
False)
self.ell_move_act.start()
def __init__(self, arm):
self.time_step = 1. / 20.
self.gripper_rot = np.pi
self.arm = arm
self.ell_traj_behavior = EPC("ellipsoid_traj")
self.ell_space = EllipsoidSpace(1)
self.tf_list = tf.TransformListener()
self.found_params = False
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.start_pub = rospy.Publisher("/start_pose", PoseStamped)
self.end_pub = rospy.Publisher("/end_pose", PoseStamped)
self.ctrl_switcher = ControllerSwitcher()
self.ell_cmd_lock = Lock()
self.params_lock = Lock()
self.ell_ep = None
self.action_preempted = False
self.ell_move_act = actionlib.SimpleActionServer("/ellipsoid_move", EllipsoidMoveAction,
self.command_move_exec, False)
self.ell_move_act.start()
class EllipsoidController(object):
def __init__(self, arm):
self.time_step = 1. / 20.
self.gripper_rot = np.pi
self.arm = arm
self.ell_traj_behavior = EPC("ellipsoid_traj")
self.ell_space = EllipsoidSpace(1)
self.tf_list = tf.TransformListener()
self.found_params = False
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.start_pub = rospy.Publisher("/start_pose", PoseStamped)
self.end_pub = rospy.Publisher("/end_pose", PoseStamped)
self.ctrl_switcher = ControllerSwitcher()
self.ell_cmd_lock = Lock()
self.params_lock = Lock()
self.ell_ep = None
self.action_preempted = False
self.ell_move_act = actionlib.SimpleActionServer("/ellipsoid_move", EllipsoidMoveAction,
self.command_move_exec, False)
self.ell_move_act.start()
def read_params(self, e_params):
with self.params_lock:
self.ell_space.load_ell_params(e_params)
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
if not self.found_params:
rospy.loginfo("[ellipsoid_controller] Found params from /ellipsoid_params")
self.reset_ell_ep()
self.found_params = True
def reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot, ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
# def reset_arm_orientation(self, duration=10.):
# if not ell_ep:
# rospy.logerr("Haven't gotten parameters from /ellipsoid_params")
# return
# with self.ell_cmd_lock:
# num_samps = duration / self.time_step
# cur_pose = self.arm.get_end_effector_pose()
# ell_pose = self.robot_ellipsoidal_pose(*(self.ell_ep + [self.gripper_rot]))
# adjust_traj = self.arm.interpolate_ep(cur_pose, ell_pose, min_jerk_traj(num_samps))
# ep_traj_control = EPTrajectoryControl(self.arm, adjust_traj)
# self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cur_pose))
# self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", ell_pose))
# self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
def command_stop(self):
self.ell_traj_behavior.stop_epc = True
def command_move_exec(self, req):
with self.params_lock:
with self.ell_cmd_lock:
change_ep = np.array([req.change_latitude, req.change_longitude, req.change_height])
abs_ep_sel = np.array([req.absolute_latitude, req.absolute_longitude, req.absolute_height])
rospy.loginfo("Commanding ellipsoidal move: (%f, %f, %f), Abs: (%d, %d, %d)" %
(change_ep[0], change_ep[1], change_ep[2],
abs_ep_sel[0], abs_ep_sel[1], abs_ep_sel[2]))
self.arm.reset_ep()
self.reset_ell_ep()
ell_f = np.where(abs_ep_sel, change_ep, self.ell_ep + change_ep)
if req.duration == 0:
self.execute_trajectory(ell_f, req.gripper_rot)
else:
self.execute_trajectory(ell_f, req.gripper_rot, req.duration)
self.ell_move_act.set_succeeded(EllipsoidMoveResult(*self.ell_ep))
def _check_preempt(self, timer_event):
if self.ell_move_act.is_preempt_requested():
self.ell_traj_behavior.stop_epc = True
self.action_preempted = True
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [self.robot_ellipsoidal_pose(ell_traj[0,i], ell_traj[1,i], ell_traj[2,i],
gripper_rot, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(), ell_pose_traj[-1],
t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1]) for i in range(num_samps)]
self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[0]))
self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
class EllipsoidController(object):
def __init__(self, arm):
self.time_step = 1. / 20.
self.gripper_rot = np.pi
self.arm = arm
self.ell_traj_behavior = EPC("ellipsoid_traj")
self.ell_space = EllipsoidSpace(1)
self.tf_list = tf.TransformListener()
self.found_params = False
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.start_pub = rospy.Publisher("/start_pose", PoseStamped)
self.end_pub = rospy.Publisher("/end_pose", PoseStamped)
self.ctrl_switcher = ControllerSwitcher()
self.ell_cmd_lock = Lock()
self.params_lock = Lock()
self.ell_ep = None
self.action_preempted = False
self.ell_move_act = actionlib.SimpleActionServer(
"/ellipsoid_move", EllipsoidMoveAction, self.command_move_exec,
False)
self.ell_move_act.start()
def read_params(self, e_params):
with self.params_lock:
self.ell_space.load_ell_params(e_params)
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
if not self.found_params:
rospy.loginfo(
"[ellipsoid_controller] Found params from /ellipsoid_params"
)
self.reset_ell_ep()
self.found_params = True
def reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time,
rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot,
ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
# def reset_arm_orientation(self, duration=10.):
# if not ell_ep:
# rospy.logerr("Haven't gotten parameters from /ellipsoid_params")
# return
# with self.ell_cmd_lock:
# num_samps = duration / self.time_step
# cur_pose = self.arm.get_end_effector_pose()
# ell_pose = self.robot_ellipsoidal_pose(*(self.ell_ep + [self.gripper_rot]))
# adjust_traj = self.arm.interpolate_ep(cur_pose, ell_pose, min_jerk_traj(num_samps))
# ep_traj_control = EPTrajectoryControl(self.arm, adjust_traj)
# self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cur_pose))
# self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", ell_pose))
# self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
def command_stop(self):
self.ell_traj_behavior.stop_epc = True
def command_move_exec(self, req):
with self.params_lock:
with self.ell_cmd_lock:
change_ep = np.array([
req.change_latitude, req.change_longitude,
req.change_height
])
abs_ep_sel = np.array([
req.absolute_latitude, req.absolute_longitude,
req.absolute_height
])
rospy.loginfo(
"Commanding ellipsoidal move: (%f, %f, %f), Abs: (%d, %d, %d)"
% (change_ep[0], change_ep[1], change_ep[2], abs_ep_sel[0],
abs_ep_sel[1], abs_ep_sel[2]))
self.arm.reset_ep()
self.reset_ell_ep()
ell_f = np.where(abs_ep_sel, change_ep,
self.ell_ep + change_ep)
if req.duration == 0:
self.execute_trajectory(ell_f, req.gripper_rot)
else:
self.execute_trajectory(ell_f, req.gripper_rot,
req.duration)
self.ell_move_act.set_succeeded(
EllipsoidMoveResult(*self.ell_ep))
def _check_preempt(self, timer_event):
if self.ell_move_act.is_preempt_requested():
self.ell_traj_behavior.stop_epc = True
self.action_preempted = True
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(
np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame",
cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [
self.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1, i],
ell_traj[2, i], gripper_rot,
ell_frame_mat)
for i in range(ell_traj.shape[1])
]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(),
ell_pose_traj[-1], t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1])
for i in range(num_samps)]
self.start_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[0]))
self.end_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.found_params = False
class HeadMarkers(object):
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.found_params = False
def read_params(self, e_params):
self.ell_space.load_ell_params(e_params)
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
if not self.found_params:
rospy.loginfo("[head_markers] Found params from /ellipsoid_params")
self.found_params = True
def create_eye_marker(self, pose, m_id, color=ColorRGBA(1., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = eye_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def create_mouth_marker(self, pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = mouth_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = ear_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def get_head(self):
if not self.found_params:
return
head_array = MarkerArray()
head_array.markers.append(
self.create_eye_marker(self.get_head_pose(l_eye_loc), 0))
head_array.markers.append(
self.create_eye_marker(self.get_head_pose(r_eye_loc), 1))
head_array.markers.append(
self.create_mouth_marker(self.get_head_pose(mouth_loc), 2))
head_array.markers.append(
self.create_ear_marker(self.get_head_pose(l_ear_loc), 3))
head_array.markers.append(
self.create_ear_marker(self.get_head_pose(r_ear_loc), 4))
return head_array
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
return pos, rot
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.head_pose_srv = rospy.Service("/get_head_pose", GetHeadPose, self.get_head_pose_srv)
self.lock_ell = False
self.found_params = False
class HeadToolPoseServer(object):
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams, self.read_params)
self.head_pose_srv = rospy.Service("/get_head_pose", GetHeadPose, self.get_head_pose_srv)
self.lock_ell = False
self.found_params = False
# self.tmp_pub = rospy.Publisher("/toolpose", PoseStamped)
def lock_ell_model(self, lock_model):
self.lock_ell = lock_model
def read_params(self, e_params):
if not self.lock_ell:
self.ell_space.load_ell_params(e_params)
if USE_ELLIPSE_FRAME:
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
self.found_params = True
def get_many_vectors(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
for lat in np.linspace(0, np.pi, 10):
color.g += 0.1
color.b = 0
for lon in np.linspace(0, 2 * np.pi, 10):
color.b += 0.1
coords.append((lat, lon, 1, i, copy.copy(color)))
i += 1
arrows.markers = [
create_arrow_marker(self.ell_space.ellipsoidal_to_pose(lat, lon, height), i, clr)
for lat, lon, height, i, clr in coords
]
return arrows
def get_pose_markers(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0.0, 1.0, 0.0, 1.0)
for name in head_poses:
arrows.markers.append(create_arrow_marker(self.get_head_pose(name), i, color))
i += 1
return arrows
def get_head_pose(self, name, gripper_rot=0.0):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, "rzyx")[:3, :3]
return pos, rot
def get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
# self.tmp_pub.publish(pose_stamped)
return pose_stamped
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.found_params = False
class HeadMarkers(object):
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.found_params = False
def read_params(self, e_params):
self.ell_space.load_ell_params(e_params)
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
if not self.found_params:
rospy.loginfo("[head_markers] Found params from /ellipsoid_params")
self.found_params = True
def create_eye_marker(self, pose, m_id, color=ColorRGBA(1., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = eye_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def create_mouth_marker(self, pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = mouth_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = ear_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def get_head(self):
if not self.found_params:
return
head_array = MarkerArray()
head_array.markers.append(
self.create_eye_marker(self.get_head_pose(l_eye_loc), 0))
head_array.markers.append(
self.create_eye_marker(self.get_head_pose(r_eye_loc), 1))
head_array.markers.append(
self.create_mouth_marker(self.get_head_pose(mouth_loc), 2))
head_array.markers.append(
self.create_ear_marker(self.get_head_pose(l_ear_loc), 3))
head_array.markers.append(
self.create_ear_marker(self.get_head_pose(r_ear_loc), 4))
return head_array
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
class HeadToolPoseServer(object):
def __init__(self):
self.ell_space = EllipsoidSpace(1)
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.head_pose_srv = rospy.Service("/get_head_pose", GetHeadPose,
self.get_head_pose_srv)
self.lock_ell = False
self.found_params = False
#self.tmp_pub = rospy.Publisher("/toolpose", PoseStamped)
def lock_ell_model(self, lock_model):
self.lock_ell = lock_model
def read_params(self, e_params):
if not self.lock_ell:
self.ell_space.load_ell_params(e_params)
if USE_ELLIPSE_FRAME:
self.ell_space.center = np.mat(np.zeros((3, 1)))
self.ell_space.rot = np.mat(np.eye(3))
self.found_params = True
def get_many_vectors(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0., 1., 0., 1.)
for lat in np.linspace(0, np.pi, 10):
color.g += 0.1
color.b = 0
for lon in np.linspace(0, 2 * np.pi, 10):
color.b += 0.1
coords.append((lat, lon, 1, i, copy.copy(color)))
i += 1
arrows.markers = [
create_arrow_marker(
self.ell_space.ellipsoidal_to_pose(lat, lon, height), i, clr)
for lat, lon, height, i, clr in coords
]
return arrows
def get_pose_markers(self):
arrows = MarkerArray()
coords = []
i = 0
color = ColorRGBA(0., 1., 0., 1.)
for name in head_poses:
arrows.markers.append(
create_arrow_marker(self.get_head_pose(name), i, color))
i += 1
return arrows
def get_head_pose(self, name, gripper_rot=0.):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConverter.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'rzyx')[:3, :3]
return pos, rot
def get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
#self.tmp_pub.publish(pose_stamped)
return pose_stamped