def se2_from_se3(mat):
#print 'mat\n', mat, mat.shape
t, r = tfu.matrix_as_tf(mat)
x = t[0]
y = t[1]
theta = tr.euler_from_quaternion(r)[2]
return x,y,theta
def se2_from_se3(mat):
#print 'mat\n', mat, mat.shape
t, r = tfu.matrix_as_tf(mat)
x = t[0]
y = t[1]
theta = tr.euler_from_quaternion(r)[2]
return x, y, theta
def make_target_marker(self): #, scale=.2, color=stdm.ColorRGBA(.5,.5,.5,.5)):
if self.is_current_task_frame:
scale = .3
color = stdm.ColorRGBA(1,0,0,.5)
else:
scale = .2
color = stdm.ColorRGBA(.5,.5,.5,.5)
name = 'target_' + self.tagid
if not self.tag_database.has_id(self.tagid):
return
p_ar = self.tag_database.get(self.tagid)['target_location']
m_ar = tfu.tf_as_matrix(p_ar)
map_T_ar = tfu.transform('map', self.tagid, self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_ar * m_ar)
pose = p_map
int_marker = interactive_marker(name, (pose[0], (0,0,0,1)), scale)
int_marker.header.frame_id = 'map' #self.tagid
int_marker.description = ''
int_marker.controls.append(make_sphere_control(name, scale/2.))
int_marker.controls[0].markers[0].color = color
int_marker.controls += make_directional_controls(name)
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.server_lock.release()
self.target_marker = int_marker
if self.menu != None:
self._make_menu()
def get_current_pose(self):
frame = str(self.frame_box.currentText())
self.tf_listener.waitForTransform(frame, ROBOT_FRAME_NAME, rospy.Time(), rospy.Duration(2.))
p_base = tfu.transform(frame, ROBOT_FRAME_NAME, self.tf_listener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
p_base_tf = tfu.matrix_as_tf(p_base)
for value, vr in zip(p_base_tf[0], [self.xline, self.yline, self.zline]):
vr.setValue(value)
for value, vr in zip(tr.euler_from_quaternion(p_base_tf[1]), [self.phi_line, self.theta_line, self.psi_line]):
vr.setValue(np.degrees(value))
def toggle_point(self):
if self.has_point:
self.server.erase(self.target_marker.name)
else:
p_ar = self.tag_database.get(self.tagid)['target_location']
m_ar = tfu.tf_as_matrix(p_ar)
map_T_ar = tfu.transform('map', self.tagid, self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_ar * m_ar)
self.make_target_marker('target_' + self.tagid, p_map)
self.server.applyChanges()
self.has_point = not self.has_point
def toggle_point(self):
if self.has_point:
self.server.erase(self.target_marker.name)
else:
p_ar = self.tag_database.get(self.tagid)['target_location']
m_ar = tfu.tf_as_matrix(p_ar)
map_T_ar = tfu.transform('map', self.tagid, self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_ar * m_ar)
self.make_target_marker('target_' + self.tagid, p_map)
self.server.applyChanges()
self.has_point = not self.has_point
def process_feedback(self, feedback):
name = feedback.marker_name
ar_match = re.search('^ar_', name)
target_match = re.search('^target_', name)
if ar_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.BUTTON_CLICK:
self.toggle_point()
if target_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.POSE_UPDATE:
m_ar = tfu.tf_as_matrix(pose_to_tup(feedback.pose))
ar_T_map = tfu.transform(self.tagid, 'map', self.tf_listener, t=0)
p_ar = tfu.matrix_as_tf(ar_T_map * m_ar)
self.tag_database.update_target_location(self.tagid, p_ar)
def process_feedback(self, feedback):
name = feedback.marker_name
ar_match = re.search('^ar_', name)
target_match = re.search('^target_', name)
if ar_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.BUTTON_CLICK:
self.toggle_point()
if target_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.POSE_UPDATE:
m_ar = tfu.tf_as_matrix(pose_to_tup(feedback.pose))
ar_T_map = tfu.transform(self.tagid, 'map', self.tf_listener, t=0)
p_ar = tfu.matrix_as_tf(ar_T_map * m_ar)
self.tag_database.update_target_location(self.tagid, p_ar)
def get_current_pose(self):
frame = str(self.frame_box.currentText())
self.tf_listener.waitForTransform(frame, ROBOT_FRAME_NAME,
rospy.Time(), rospy.Duration(2.))
p_base = tfu.transform(frame, ROBOT_FRAME_NAME, self.tf_listener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
p_base_tf = tfu.matrix_as_tf(p_base)
for value, vr in zip(p_base_tf[0],
[self.xline, self.yline, self.zline]):
vr.setValue(value)
for value, vr in zip(tr.euler_from_quaternion(p_base_tf[1]),
[self.phi_line, self.theta_line, self.psi_line]):
vr.setValue(np.degrees(value))
def learnable_behavior_menu_cb(self, menu_item, full_action_path, int_feedback):
clicked_action = full_action_path
folder, action_name = pt.split(full_action_path)
all_actions = glob.glob(pt.join(folder, '*'))
nonmatchings = []
for a in all_actions:
if a != clicked_action:
nonmatchings.append(a)
if len(nonmatchings) > 1:
rospy.logerr('Found too many items in %s. There can only be two behaviors.' % folder)
return
clicked_complement = nonmatchings[0]
rospy.loginfo('clicked %s, complement is %s' % (clicked_action, clicked_complement))
p_tll = tfu.tf_as_matrix(HEAD_MARKER_LOC)
self.tf_listener.waitForTransform('map', 'torso_lift_link', rospy.Time.now(), rospy.Duration(10))
map_T_tll = tfu.transform('map', 'torso_lift_link', self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_tll * p_tll)
#Create two markers
actionid = self.action_marker_manager.create_action('map', tagid=None, loc=p_map,
name=pt.split(clicked_action)[1])
self.action_marker_manager._action_selection_menu_cb(actionid,
menu_item, clicked_action, int_feedback)
cactionid = self.action_marker_manager.create_action('map', tagid=None, loc=p_map,
name=pt.split(clicked_complement)[1])
self.action_marker_manager._action_selection_menu_cb(cactionid,
menu_item, clicked_complement, int_feedback)
self.action_marker_manager.marker_db.set_property(actionid, 'complement', cactionid)
self.action_marker_manager.marker_db.set_property(cactionid, 'complement', actionid)
def marker_cb(self, feedback):
name = feedback.marker_name
ar_match = re.search('^ar_', name)
target_match = re.search('^target_', name)
#if feedback.event_type == ims.InteractiveMarkerFeedback.MOUSE_DOWN:
# print name, feedback
print 'called', feedback_to_string(feedback.event_type), feedback.marker_name, feedback.control_name
if ar_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.MOUSE_DOWN:
#print 'lock acq'
self.toggle_point()
#print 'toggle!'
if target_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.POSE_UPDATE:
m_ar = tfu.tf_as_matrix(pose_to_tup(feedback.pose))
ar_T_map = tfu.transform(self.tagid, 'map', self.tf_listener, t=0)
p_ar = tfu.matrix_as_tf(ar_T_map * m_ar)
self.tag_database.update_target_location(self.tagid, p_ar)
sphere_match = re.search('_sphere$', feedback.control_name)
menu_match = re.search('_menu$', feedback.control_name)
if ((sphere_match != None) or (menu_match != None)) and target_match != None and feedback.event_type == ims.InteractiveMarkerFeedback.MOUSE_DOWN:
self.frame_selected_cb(self.tagid)
def _to_map_frame(self, p_ar):
m_ar = tfu.tf_as_matrix(p_ar)
self.tf_listener.waitForTransform('map', self.frame, rospy.Time.now(), rospy.Duration(10))
map_T_ar = tfu.transform('map', self.frame, self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_ar * m_ar)
return p_map