def execute(self, userdata):
#Create goal and send it up here
self.tf_listener.waitForTransform(self.CONTROL_FRAME, self.pose_stamped.header.frame_id, rospy.Time(0), rospy.Duration(10.))
bl_T_frame = tfu.tf_as_matrix(self.tf_listener.lookupTransform(self.CONTROL_FRAME, self.pose_stamped.header.frame_id, rospy.Time(0)))
#print 'control_T_frame\n', bl_T_frame
trans = np.array([self.pose_stamped.pose.position.x, self.pose_stamped.pose.position.y, self.pose_stamped.pose.position.z])
quat = [self.pose_stamped.pose.orientation.x, self.pose_stamped.pose.orientation.y,
self.pose_stamped.pose.orientation.z, self.pose_stamped.pose.orientation.w]
h_frame = tfu.tf_as_matrix((trans, quat))
#([self.x, self.y, 0], tr.quaternion_from_euler(0, 0, self.t)))
#print 'h_frame\n', h_frame
#print 'bl_T_frame * h_frame\n', bl_T_frame * h_frame
x, y, t = se2_from_se3(bl_T_frame * h_frame)
#print 'GOAL', x, y, np.degrees(t)
xy_goal = smb.GoXYGoal(x,y)
self.go_xy_client.send_goal(xy_goal)
result_xy = tu.monitor_goals(self, [self.go_xy_client], 'PreciseNavigateSmach', self.time_out)
if result_xy != 'succeeded':
return result_xy
ang_goal = smb.GoAngleGoal(t)
self.go_angle_client.send_goal(ang_goal)
result_ang = tu.monitor_goals(self, [self.go_angle_client], 'PreciseNavigateSmach', self.time_out)
return result_ang
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 execute(self, userdata):
#Create goal and send it up here
self.tf_listener.waitForTransform(self.CONTROL_FRAME,
self.pose_stamped.header.frame_id,
rospy.Time(0), rospy.Duration(10.))
bl_T_frame = tfu.tf_as_matrix(
self.tf_listener.lookupTransform(self.CONTROL_FRAME,
self.pose_stamped.header.frame_id,
rospy.Time(0)))
#print 'control_T_frame\n', bl_T_frame
trans = np.array([
self.pose_stamped.pose.position.x,
self.pose_stamped.pose.position.y,
self.pose_stamped.pose.position.z
])
quat = [
self.pose_stamped.pose.orientation.x,
self.pose_stamped.pose.orientation.y,
self.pose_stamped.pose.orientation.z,
self.pose_stamped.pose.orientation.w
]
h_frame = tfu.tf_as_matrix((trans, quat))
#([self.x, self.y, 0], tr.quaternion_from_euler(0, 0, self.t)))
#print 'h_frame\n', h_frame
#print 'bl_T_frame * h_frame\n', bl_T_frame * h_frame
x, y, t = se2_from_se3(bl_T_frame * h_frame)
#print 'GOAL', x, y, np.degrees(t)
xy_goal = smb.GoXYGoal(x, y)
self.go_xy_client.send_goal(xy_goal)
result_xy = tu.monitor_goals(self, [self.go_xy_client],
'PreciseNavigateSmach', self.time_out)
if result_xy != 'succeeded':
return result_xy
ang_goal = smb.GoAngleGoal(t)
self.go_angle_client.send_goal(ang_goal)
result_ang = tu.monitor_goals(self, [self.go_angle_client],
'PreciseNavigateSmach', self.time_out)
return result_ang
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 get_current_pose(self):
frame_described_in = str(self.frame_box.currentText())
arm = tu.selected_radio_button(self.arm_radio_buttons).lower()
if arm == 'right':
arm_tip_frame = VelocityPriorityMoveTool.RIGHT_TIP
else:
arm_tip_frame = VelocityPriorityMoveTool.LEFT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.))
p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0)))
trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm)
for value, vr in zip(trans, [self.xline, self.yline, self.zline]):
vr.setValue(value)
for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]):
vr.setValue(np.degrees(value))
def get_current_pose(self):
frame_described_in = str(self.frame_box.currentText())
arm = tu.selected_radio_button(self.arm_radio_buttons).lower()
if arm == 'right':
arm_tip_frame = VelocityPriorityMoveTool.RIGHT_TIP
else:
arm_tip_frame = VelocityPriorityMoveTool.LEFT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame,
rospy.Time(), rospy.Duration(2.))
p_arm = tfu.tf_as_matrix(
self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame,
rospy.Time(0)))
trans, rotation = tr.translation_from_matrix(
p_arm), tr.quaternion_from_matrix(p_arm)
for value, vr in zip(trans, [self.xline, self.yline, self.zline]):
vr.setValue(value)
for value, vr in zip(tr.euler_from_quaternion(rotation),
[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 run(self):
#Change duration to cumulative time.
if len(self.messages) == 0:
return
#put in the current pose so that we'll interpolate there
messages = []
frame_described_in = self.messages[0]['pose_stamped'].header.frame_id
if self.messages[0]['arm'] == 'left':
arm_tip_frame = VelocityPriorityMoveTool.LEFT_TIP
else:
arm_tip_frame = VelocityPriorityMoveTool.RIGHT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame,
rospy.Duration(.5))
trans, rotation = self.tf_listener.lookupTransform(
frame_described_in, arm_tip_frame, rospy.Time(0))
initial_pose = geo.PoseStamped()
initial_pose.pose.position.x = trans[0]
initial_pose.pose.position.y = trans[1]
initial_pose.pose.position.z = trans[2]
initial_pose.pose.orientation.x = rotation[0]
initial_pose.pose.orientation.y = rotation[1]
initial_pose.pose.orientation.z = rotation[2]
initial_pose.pose.orientation.w = rotation[3]
initial_pose.header = self.messages[0]['pose_stamped'].header
current_self_messages = [{
'arm': self.messages[0]['arm'],
'pose_stamped': initial_pose,
'time': self.messages[0]['time']
}]
current_self_messages = current_self_messages + self.messages
#interpolate
for msg_a, msg_b, idx_a in zip(current_self_messages[:-1],
current_self_messages[1:],
range(len(current_self_messages[:-1]))):
n_steps = int(np.floor(msg_b['time'] / self.step_resolution))
if n_steps == 0:
messages.append(msg_a)
else:
for i in range(n_steps):
ps = interpolate_pose_stamped(msg_a['pose_stamped'],
msg_b['pose_stamped'],
i / float(n_steps))
msg_i = {
'arm': msg_a['arm'],
'pose_stamped': ps,
'time': self.step_resolution
}
messages.append(msg_i)
messages.append({
'arm': self.messages[-1]['arm'],
'pose_stamped': self.messages[-1]['pose_stamped'],
'time': self.step_resolution
})
timeslp = [0.0]
for idx, message in enumerate(messages[1:]):
timeslp.append(message['time'] + timeslp[idx])
#current_pose = tfu.tf_as_matrix(self.tf_listener.lookupTransform(VelocityPriorityMoveTool.COMMAND_FRAME,
# self.controller_frame, rospy.Time(0)))
#print 'current_pose\n', current_pose
wall_start_time = rospy.get_rostime().to_time()
for t, el in zip(timeslp, messages):
#Sleep if needed
cur_time = rospy.get_rostime().to_time()
wall_time_from_start = cur_time - wall_start_time
sleep_time = (t - wall_time_from_start) - .005
#print 'sleeping for', sleep_time
if sleep_time > 0:
time.sleep(sleep_time)
if self.stop:
return
#Our command is in tip frame, but controller is operating in wrist frame
#print "tip\n", el['pose_stamped']
tip_torso = change_pose_stamped_frame(
self.tf_listener, el['pose_stamped'],
VelocityPriorityMoveTool.COMMAND_FRAME)
tll_T_tip = pose_to_mat(tip_torso.pose)
#print "tll_T_tip\n", tll_T_tip
tip_T_wrist = tfu.tf_as_matrix(
self.tf_listener.lookupTransform(self.tip_frame,
self.controller_frame,
rospy.Time(0)))
#print 'tip_T_wrist\n', tip_T_wrist
tll_T_wrist = tll_T_tip * tip_T_wrist
wrist_torso = stamp_pose(mat_to_pose(tll_T_wrist),
'torso_lift_link')
#Send
self.cart_pub.publish(wrist_torso)
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
def run(self):
#Change duration to cumulative time.
if len(self.messages) == 0:
return
#put in the current pose so that we'll interpolate there
messages = []
frame_described_in = self.messages[0]['pose_stamped'].header.frame_id
if self.messages[0]['arm'] == 'left':
arm_tip_frame = VelocityPriorityMoveTool.LEFT_TIP
else:
arm_tip_frame = VelocityPriorityMoveTool.RIGHT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Duration(.5))
trans, rotation = self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))
initial_pose = geo.PoseStamped()
initial_pose.pose.position.x = trans[0]
initial_pose.pose.position.y = trans[1]
initial_pose.pose.position.z = trans[2]
initial_pose.pose.orientation.x = rotation[0]
initial_pose.pose.orientation.y = rotation[1]
initial_pose.pose.orientation.z = rotation[2]
initial_pose.pose.orientation.w = rotation[3]
initial_pose.header = self.messages[0]['pose_stamped'].header
current_self_messages = [{'arm': self.messages[0]['arm'],
'pose_stamped': initial_pose,
'time': self.messages[0]['time']}]
current_self_messages = current_self_messages + self.messages
#interpolate
for msg_a, msg_b, idx_a in zip(current_self_messages[:-1], current_self_messages[1:], range(len(current_self_messages[:-1]))):
n_steps = int(np.floor(msg_b['time'] / self.step_resolution))
if n_steps == 0:
messages.append(msg_a)
else:
for i in range(n_steps):
ps = interpolate_pose_stamped(msg_a['pose_stamped'], msg_b['pose_stamped'], i/float(n_steps))
msg_i = {'arm': msg_a['arm'],
'pose_stamped': ps,
'time': self.step_resolution}
messages.append(msg_i)
messages.append({'arm': self.messages[-1]['arm'],
'pose_stamped': self.messages[-1]['pose_stamped'],
'time': self.step_resolution})
timeslp = [0.0]
for idx, message in enumerate(messages[1:]):
timeslp.append(message['time'] + timeslp[idx])
#current_pose = tfu.tf_as_matrix(self.tf_listener.lookupTransform(VelocityPriorityMoveTool.COMMAND_FRAME,
# self.controller_frame, rospy.Time(0)))
#print 'current_pose\n', current_pose
wall_start_time = rospy.get_rostime().to_time()
for t, el in zip(timeslp, messages):
#Sleep if needed
cur_time = rospy.get_rostime().to_time()
wall_time_from_start = cur_time - wall_start_time
sleep_time = (t - wall_time_from_start) - .005
#print 'sleeping for', sleep_time
if sleep_time > 0:
time.sleep(sleep_time)
if self.stop:
return
#Our command is in tip frame, but controller is operating in wrist frame
#print "tip\n", el['pose_stamped']
tip_torso = change_pose_stamped_frame(self.tf_listener,
el['pose_stamped'], VelocityPriorityMoveTool.COMMAND_FRAME)
tll_T_tip = pose_to_mat(tip_torso.pose)
#print "tll_T_tip\n", tll_T_tip
tip_T_wrist = tfu.tf_as_matrix(self.tf_listener.lookupTransform(self.tip_frame, self.controller_frame, rospy.Time(0)))
#print 'tip_T_wrist\n', tip_T_wrist
tll_T_wrist = tll_T_tip * tip_T_wrist
wrist_torso = stamp_pose(mat_to_pose(tll_T_wrist), 'torso_lift_link')
#Send
self.cart_pub.publish(wrist_torso)