def create_grasp_dialog(self):
if not self.grasp_dialog:
if not self.ui.ph.is_pose_set(
self.ui.program_vis.block_id,
self.ui.program_vis.get_current_item().id):
self.ui.notif(
translate(
"PickFromFeeder",
"Use robot's arm and dialog to teach pose enabling part detection."
))
else:
self.ui.notif(
translate(
"PickFromFeeder",
"Learned pose for part detection may be updated or different object type could be chosen."
))
self.grasp_dialog = DialogItem(
self.ui.scene, self.ui.width / 2, 0.1,
translate("PickFromFeeder", "Save gripper pose"), [
arm.name(self.ui.loc) + " (0)"
for arm in self.ui.rh.get_robot_arms()
], self.save_gripper_pose_cb)
for it in self.grasp_dialog.items:
it.set_enabled(False)
def create_drill_dialog(self):
if not self.drill_dialog:
arr = [
arm.name(self.ui.loc) for arm in self.ui.rh.get_robot_arms()
]
arr.append(translate("DrillPoints", "Prev pose"))
arr.append(translate("DrillPoints", "Next pose"))
self.drill_pose_idx = 0
self.drill_dialog = DialogItem(self.ui.scene, self.ui.width / 2,
0.1, self.get_drill_caption(), arr,
self.save_gripper_pose_drill_cb)
def __init__(self, *args, **kwargs):
super(VisualInspectionLearn, self).__init__(*args, **kwargs)
self.dialog = None
rp_learned, rp_id = self.ui.ph.ref_pick_learned(*self.cid)
if not rp_learned:
self.ui.notif(
translate(
"VisualInspection",
"Pick instruction (%1) has to be set first.").arg(rp_id))
self.notified = True
if self.editable:
self.ui.notif(
translate("VisualInspection",
"Now you may adjust pose for visual inspection."))
self.notified = True
self.dialog = DialogItem(
self.ui.scene, self.ui.width / 2, 0.1,
translate("VisualInspection", "Save visual inspection pose"),
[arm.name(self.ui.loc) for arm in self.ui.rh.get_robot_arms()],
self.save_pose_cb)
self.dialog_timer = QtCore.QTimer()
self.dialog_timer.timeout.connect(self.dialog_timer_tick)
self.dialog_timer.setSingleShot(True)
def learning_request_done_evt(self, status, result):
# TODO some notif
self.program_vis.learning_request_result(result.success)
if self.program_vis.editing_item:
item = self.program_vis.get_current_item()
if item.type == ProgIt.PICK_FROM_FEEDER:
self.grasp_dialog = DialogItem(self.scene, self.width / 2, 0.1,
"Save gripper pose",
["Right arm", "Left arm"],
self.save_gripper_pose_cb)
def learning_vis(self, block_id, item_id, read_only):
if not self.ph.item_requires_learning(block_id, item_id):
self.notif(translate("UICoreRos", "Item has no parameters."))
return
self.program_vis.editing_item = not read_only
# TODO Edit/Done button not visible when there is work in progress!
if block_id != self.program_vis.block_id or item_id != self.program_vis.item_id:
self.program_vis.set_active(block_id, item_id)
msg = self.ph.get_item_msg(block_id, item_id)
if self.ph.item_learned(block_id, item_id):
self.notif(
translate("UICoreRos", "This program item seems to be done"))
else:
if msg.type in [
ProgIt.PICK_FROM_POLYGON, ProgIt.PICK_FROM_FEEDER,
ProgIt.PICK_OBJECT_ID, ProgIt.PLACE_TO_POSE
]:
self.notif(
translate("UICoreRos",
"Program current manipulation task"))
if msg.type == ProgIt.PICK_FROM_POLYGON:
if not self.ph.is_object_set(block_id, item_id):
self.notif(translate("UICoreRos",
"Select object type to be picked up"),
temp=True)
else:
self.select_object_type(msg.object[0])
if self.ph.is_polygon_set(block_id, item_id):
self.add_polygon(
translate("UICoreRos", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(msg),
polygon_changed=self.polygon_changed,
fixed=read_only)
elif msg.type == ProgIt.PICK_FROM_FEEDER:
if self.state_manager.state.edit_enabled and self.grasp_dialog is None:
self.grasp_dialog = DialogItem(self.scene, self.width / 2, 0.1,
"Save gripper pose",
["Right arm", "Left arm"],
self.save_gripper_pose_cb)
if self.ph.is_object_set(block_id, item_id):
self.select_object_type(msg.object[0])
else:
self.notif(translate("UICoreRos",
"Select object type to be picked up"),
temp=True)
# TODO show pick pose somehow (arrow??)
elif msg.type == ProgIt.PICK_OBJECT_ID:
if self.ph.is_object_set(block_id, item_id):
self.select_object(msg.object[0])
else:
self.notif(translate("UICoreRos",
"Select object to be picked up"),
temp=True)
elif msg.type == ProgIt.PLACE_TO_POSE:
if not self.ph.is_object_set(block_id, msg.ref_id[0]):
self.notif(
translate("UICoreRos",
"Select object to be picked up in ID=") +
str(msg.ref_id[0]))
else:
# TODO what to do with more than 1 reference?
ref_msg = self.ph.get_item_msg(block_id, msg.ref_id[0])
if ref_msg.type == ProgIt.PICK_OBJECT_ID:
obj = self.get_object(ref_msg.object[0])
object_type = obj.object_type
object_id = obj.object_id
self.select_object(ref_msg.object[0])
else:
object_type = self.art.get_object_type(ref_msg.object[0])
object_id = None
self.select_object_type(ref_msg.object[0])
if self.ph.is_object_set(block_id, msg.ref_id[0]):
if self.ph.is_pose_set(block_id, item_id):
if object_type is not None:
self.add_place(translate("UICoreRos",
"OBJECT PLACE POSE"),
msg.pose[0],
object_type,
object_id,
place_cb=self.place_pose_changed,
fixed=read_only)
else:
self.notif(
translate("UICoreRos",
"Set where to place picked object"))
self.add_place(translate("UICoreRos",
"OBJECT PLACE POSE"),
self.get_def_pose(),
object_type,
object_id,
place_cb=self.place_pose_changed,
fixed=read_only)
elif msg.type == ProgIt.PLACE_TO_GRID:
ref_msg = self.program_vis.get_ref_item(
msg.ref_id) # obtaining reference instruction
if ref_msg.type == ProgIt.PICK_OBJECT_ID:
obj = self.get_object(ref_msg.object[0])
object_type = obj.object_type
self.program_vis.get_current_item().object[0] = obj.object_id
else:
object_type = self.art.get_object_type(ref_msg.object[0])
self.program_vis.get_current_item(
).object[0] = ref_msg.object[0]
self.notif(translate("UICoreRos", "Place grid"))
self.add_square(
translate("UICoreRos", "PLACE SQUARE GRID"),
self.width / 2,
self.height / 2,
0.1,
0.075,
object_type,
msg.pose,
grid_points=conversions.get_pick_polygon_points(msg),
square_changed=self.square_changed,
fixed=read_only)
def interface_state_evt(self, old_state, state, flags):
system_state_changed = old_state.system_state != state.system_state
if system_state_changed:
rospy.logdebug("New system state: " + str(state.system_state) +
", was: " + str(old_state.system_state))
self.clear_all(True)
if state.error_severity == InterfaceState.NONE and self.program_error_dialog is not None:
# hide dialog
self.scene.removeItem(self.program_error_dialog)
self.program_error_dialog = None
# display info/warning/error if there is any - only once (on change)
if state.error_severity != old_state.error_severity:
if state.error_severity == InterfaceState.INFO:
self.notif(translate("UICoreRos", "Error occurred: ") +
error_strings.get_error_string(state.error_code),
temp=True)
elif state.error_severity == InterfaceState.WARNING:
# TODO translate error number to error message
self.program_error_dialog = DialogItem(
self.scene, self.width / 2, 0.1,
translate("UICoreRos", "Handle error: ") +
error_strings.get_error_string(state.error_code), [
translate("UICoreRos", "Try again"),
translate("UICoreRos", "Skip instruction"),
translate("UICoreRos", "Fail instruction"),
translate("UICoreRos", "End program")
], self.program_error_dialog_cb)
# TODO what to do with SEVERE?
if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED:
if system_state_changed:
self.notif(translate("UICoreRos", "The program is done."))
elif state.system_state == InterfaceState.STATE_IDLE:
if system_state_changed:
self.show_program_list()
elif state.system_state == InterfaceState.STATE_LEARNING:
self.state_learning(old_state, state, flags, system_state_changed)
elif state.system_state in [
InterfaceState.STATE_PROGRAM_RUNNING,
InterfaceState.STATE_PROGRAM_STOPPED
]:
self.state_running(old_state, state, flags, system_state_changed)
class PickFromFeederLearn(PickFromFeeder):
def __init__(self, *args, **kwargs):
super(PickFromFeederLearn, self).__init__(*args, **kwargs)
QtCore.QObject.connect(self, QtCore.SIGNAL('objects_raw'),
self.object_raw_cb_evt)
self.grasp_dialog = None
self.grasp_dialog_timer = QtCore.QTimer()
self.grasp_dialog_timer.timeout.connect(self.grasp_dialog_timer_tick)
self.grasp_dialog_timer.start(1000)
# TODO delegate getting detections per frame to object helper
self.obj_raw_sub = rospy.Subscriber('/art/object_detector/object',
InstancesArray,
self.object_raw_cb,
queue_size=1)
self.objects_by_sensor = {}
if self.ui.ph.is_object_set(*self.cid):
self.ui.select_object_type(self.ui.ph.get_object(*self.cid)[0][0])
if self.editable:
self.create_grasp_dialog()
else:
if self.editable:
self.ui.notif(
translate(
"PickFromFeeder",
"Select object type to be picked up by tapping on its outline."
))
def create_grasp_dialog(self):
if not self.grasp_dialog:
if not self.ui.ph.is_pose_set(
self.ui.program_vis.block_id,
self.ui.program_vis.get_current_item().id):
self.ui.notif(
translate(
"PickFromFeeder",
"Use robot's arm and dialog to teach pose enabling part detection."
))
else:
self.ui.notif(
translate(
"PickFromFeeder",
"Learned pose for part detection may be updated or different object type could be chosen."
))
self.grasp_dialog = DialogItem(
self.ui.scene, self.ui.width / 2, 0.1,
translate("PickFromFeeder", "Save gripper pose"), [
arm.name(self.ui.loc) + " (0)"
for arm in self.ui.rh.get_robot_arms()
], self.save_gripper_pose_cb)
for it in self.grasp_dialog.items:
it.set_enabled(False)
def save_gripper_pose_cb(self, idx):
ps = self.ui.rh.get_robot_arms()[idx].get_pose()
if ps:
self.ui.notif(translate("PickFromFeeder", "Gripper pose stored."),
temp=True)
self.ui.notify_info()
self.ui.program_vis.set_pose(ps)
self.grasp_dialog.items[idx].set_caption(
translate("PickFromFeeder", "Stored"))
else:
self.ui.notif(translate("PickFromFeeder",
"Failed to get gripper pose."),
temp=True)
self.grasp_dialog.items[idx].set_caption(
translate("PickFromFeeder", "Failed"))
self.ui.notify_warn()
self.grasp_dialog.items[idx].set_enabled(False)
def object_raw_cb_evt(self, msg):
cnt = 0
for obj in msg.instances:
if obj.object_type in self.ui.selected_object_types:
# this mainly serves for detection of objects in feeder so let's count only objects not on table
o = self.ui.get_object(obj.object_id)
if o and o.on_table:
continue
cnt += 1
self.objects_by_sensor[msg.header.frame_id] = [cnt, msg.header.stamp]
def object_selected(self, obj, selected, msg):
# this type of object is already set
if msg.object and obj.object_type.name == msg.object[0]:
self.logdebug("object type " + obj.object_type.name +
" already selected")
return
else:
# TODO remove previously inserted polygon, do not insert new
# place
self.logdebug("selecting new object type: " + obj.object_type.name)
if obj.object_type.name != self.ui.ph.get_object(
self.ui.program_vis.block_id, msg.id)[0][0]:
self.ui.program_vis.clear_poses()
self.ui.program_vis.set_object(obj.object_type.name)
self.ui.select_object_type(obj.object_type.name)
self.create_grasp_dialog()
def grasp_dialog_timer_tick(self):
now = rospy.Time.now()
for k, v in self.objects_by_sensor.iteritems():
if now - v[1] > rospy.Duration(1.0):
v[0] = 0
if self.grasp_dialog:
if now - self.ui.program_vis.get_current_item(
).pose[0].header.stamp < rospy.Duration(3.0):
return
for idx, arm in enumerate(self.ui.rh.get_robot_arms()):
try:
cnt = self.objects_by_sensor[arm.camera_link][0]
except KeyError:
cnt = 0
self.grasp_dialog.items[idx].set_enabled(cnt == 1)
self.grasp_dialog.items[idx].set_caption(
arm.name(self.ui.loc) + " (" + str(cnt) + ")")
def object_raw_cb(self, msg):
self.emit(QtCore.SIGNAL('objects_raw'), msg)
def cleanup(self):
self.ui.scene.removeItem(self.grasp_dialog)
self.grasp_dialog = None
return ()
def learning_done(self):
if self.grasp_dialog:
self.ui.scene.removeItem(self.grasp_dialog)
self.grasp_dialog = None
def detected_objects(self, msg):
if self.grasp_dialog:
sel_obj_type = self.ui.ph.get_object(*self.cid)[0][0]
if self.grasp_dialog:
for obj in msg.instances:
if obj.object_type == sel_obj_type:
self.grasp_dialog.set_enabled(True)
break
else:
self.grasp_dialog.set_enabled(False)
def interface_state_evt(self, our_state, state, flags):
# display info/warning/error if there is any - only once (on change)
if state.error_severity != InterfaceState.NONE and our_state.error_severity != state.error_severity:
# TODO translate error number to error message
self.notif(translate("UICoreRos", "Error occured: ") +
error_strings.get_error_string(state.error_code),
temp=True)
if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED or state.system_state == InterfaceState.STATE_IDLE:
self.clear_all()
self.notif(translate("UICoreRos", "The program is done."),
temp=True)
if state.system_state == InterfaceState.STATE_PROGRAM_FINISHED:
self.fsm.tr_program_finished()
elif state.system_state == InterfaceState.STATE_LEARNING:
# TODO !!
pass
elif state.system_state == InterfaceState.STATE_PROGRAM_RUNNING:
if state.error_severity == InterfaceState.WARNING and self.program_error_dialog is None:
# TODO translate error number to error message
self.program_error_dialog = DialogItem(
self.scene, self.width / 2, 0.1,
translate("UICoreRos", "Handle error: ") +
error_strings.get_error_string(state.error_code), [
translate("UICoreRos", "Try again"),
translate("UICoreRos", "Skip instruction"),
translate("UICoreRos", "Fail instruction"),
translate("UICoreRos", "End program")
], self.program_error_dialog_cb)
self.clear_all()
if self.fsm.state != 'running':
self.fsm.tr_running()
# TODO handle this - display ProgramItem (if it's not already
# displayed), load proper program (if not loaded) etc.
return
self.program_vis.set_active(state.block_id,
state.program_current_item.id)
it = state.program_current_item
if it.type == ProgIt.GET_READY:
self.notif(translate("UICoreRos", "Robot is getting ready"))
elif it.type == ProgIt.WAIT_FOR_USER:
self.notif(translate("UICoreRos", "Waiting for user"))
elif it.type == ProgIt.WAIT_UNTIL_USER_FINISHES:
self.notif(translate("UICoreRos",
"Waiting for user to finish"))
elif it.type == ProgIt.PICK_FROM_POLYGON:
obj_id = None
try:
obj_id = flags["SELECTED_OBJECT_ID"]
except KeyError:
rospy.logerr(
"PICK_FROM_POLYGON: SELECTED_OBJECT_ID flag not set")
if obj_id is not None:
self.select_object(obj_id)
obj = self.get_object(obj_id) # TODO notif - object type
self.notif(
translate("UICoreRos", "Going to pick object ID ") +
obj_id + translate("UICoreRos", " of type ") +
obj.object_type.name +
translate("UICoreRos", " from polygon."))
self.add_polygon(
translate("UICoreRos", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(it),
fixed=True)
elif it.type == ProgIt.PICK_FROM_FEEDER:
# TODO PICK_FROM_FEEDER
pass
elif it.type == ProgIt.PICK_OBJECT_ID:
self.notif(
translate("UICoreRos", "Picking object with ID=") +
it.object[0])
self.select_object(it.object[0])
elif it.type == ProgIt.PLACE_TO_POSE:
try:
obj_id = flags["SELECTED_OBJECT_ID"]
except KeyError:
rospy.logerr(
"PLACE_TO_POSE: SELECTED_OBJECT_ID flag not set")
return
obj = self.get_object(obj_id)
if obj is not None:
self.add_place(translate("UICoreRos", "OBJECT PLACE POSE"),
it.pose[0],
obj.object_type,
obj_id,
fixed=True)
else:
# TODO what to do if brain wants to manipulate with
# non-existent object?
pass
class DrillPointsLearn(DrillPoints):
def __init__(self, *args, **kwargs):
super(DrillPointsLearn, self).__init__(*args, **kwargs)
self.drill_dialog = None
self.drill_pose_idx = 0
# TODO check if object is to be set here or somewhere else!
if self.ui.ph.is_object_set(*self.cid):
self.ui.select_object_type(self.ui.ph.get_object(*self.cid)[0][0])
if self.editable:
self.create_drill_dialog()
if self.ui.ph.is_polygon_set(*self.cid):
polygons = self.ui.ph.get_polygon(*self.cid)[0]
self.ui.add_polygon(
translate("DrillPoints", "OBJECTS TO BE DRILLED"),
poly_points=conversions.get_pick_polygon_points(polygons),
polygon_changed=self.ui.polygon_changed,
fixed=not self.editable)
else:
# TODO pokud nema byt nastaveny v teto instrukci - rict kde je potreba ho nastavit
# TODO pokud tam neni vybrany, ani nedovolit editaci - neni co editovat
if self.editable:
self.ui.notif(
translate("DrillPoints",
"Select object type to be drilled"))
def cleanup(self):
self.ui.scene.removeItem(self.drill_dialog)
self.drill_dialog = None
return ()
def object_selected(self, obj, selected, msg):
# polygon is not from some other instruction (through ref_id)
# and new object type was selected
if obj.object_type.name != self.ui.ph.get_object(*self.cid)[0][0]:
if msg.polygon:
self.ui.remove_scene_items_by_type(PolygonItem)
poly_points = []
self.ui.program_vis.set_object(obj.object_type.name)
self.ui.select_object_type(obj.object_type.name)
# TODO avoid code duplication with PICK_FROM_POLYGON
for ob in self.ui.get_scene_items_by_type(ObjectItem):
if ob.object_type.name != obj.object_type.name:
continue
# TODO refactor somehow (into ObjectItem?)
if not ob.on_table or ob.position[0] < 0 or ob.position[0] > self.ui.width or ob.position[1] < 0 \
or ob.position[1] > self.ui.height:
continue
sbr = ob.sceneBoundingRect()
w = ob.pix2m(sbr.width())
h = ob.pix2m(sbr.height())
# TODO limit to scene size?
poly_points.append(
(ob.position[0] + w / 2.0, ob.position[1] + h / 2.0))
poly_points.append(
(ob.position[0] - w / 2.0, ob.position[1] - h / 2.0))
poly_points.append(
(ob.position[0] + w / 2.0, ob.position[1] - h / 2.0))
poly_points.append(
(ob.position[0] - w / 2.0, ob.position[1] + h / 2.0))
# TODO polygon_changed should be now set for add_polygon?
self.ui.add_polygon(translate("DrillPoints",
"OBJECTS TO BE DRILLED"),
poly_points,
polygon_changed=self.ui.polygon_changed)
self.ui.notif(
translate(
"DrillPoints",
"Check and adjust area with objects to be drilled. Then use robot arm to set drill poses."
))
self.ui.program_vis.clear_poses()
self.create_drill_dialog()
def detected_objects(self, msg):
if self.drill_dialog:
sel_obj_type = self.ui.ph.get_object(*self.cid)[0][0]
polygon = self.ui.ph.get_polygon(*self.cid)[0][0]
pp = []
for point in polygon.polygon.points:
pp.append([point.x, point.y])
pp.append([0, 0])
pol = mplPath.Path(np.array(pp), closed=True)
for obj in msg.instances:
if obj.object_type == sel_obj_type and pol.contains_point(
[obj.pose.position.x, obj.pose.position.y]):
self.drill_dialog.set_enabled(True)
break
else:
self.drill_dialog.set_enabled(False)
def learning_done(self):
if self.drill_dialog:
self.ui.scene.removeItem(self.drill_dialog)
self.drill_dialog = None
def get_drill_caption(self):
return translate(
"DrillPoints",
"Save gripper pose (%1/%2)").arg(self.drill_pose_idx + 1).arg(
self.ui.program_vis.get_poses_count())
def create_drill_dialog(self):
if not self.drill_dialog:
arr = [
arm.name(self.ui.loc) for arm in self.ui.rh.get_robot_arms()
]
arr.append(translate("DrillPoints", "Prev pose"))
arr.append(translate("DrillPoints", "Next pose"))
self.drill_pose_idx = 0
self.drill_dialog = DialogItem(self.ui.scene, self.ui.width / 2,
0.1, self.get_drill_caption(), arr,
self.save_gripper_pose_drill_cb)
def save_gripper_pose_drill_cb(self, idx):
# "Right arm", "Left arm", "Prev pose", "Next pose"
if idx in [2, 3]:
if idx == 3:
self.drill_pose_idx += 1
if self.drill_pose_idx >= self.ui.program_vis.get_poses_count(
):
self.drill_pose_idx = 0
else:
self.drill_pose_idx -= 1
if self.drill_pose_idx < 0:
self.drill_pose_idx = self.ui.program_vis.get_poses_count(
) - 1
self.ui.drill_dialog.set_caption(self.get_drill_caption())
return
ps = self.ui.rh.get_robot_arms()[idx].get_pose()
if not ps:
self.ui.notif(translate("DrillPoints",
"Failed to get gripper pose."),
temp=True,
message_type=NotifyUserRequest.WARN)
return
assert ps.header.frame_id == "marker"
obj_type = self.ui.ph.get_object(*self.cid)[0][0]
polygon = self.ui.ph.get_polygon(*self.cid)[0][0]
pp = []
for point in polygon.polygon.points:
pp.append([point.x, point.y])
pp.append([0, 0])
pol = mplPath.Path(np.array(pp), closed=True)
dist = None
c_obj = None
for obj in self.ui.get_scene_items_by_type(ObjectItem):
# skip objects of different type or outside of polygon
if obj.object_type.name != obj_type or not pol.contains_point(
[obj.position[0], obj.position[1]]):
continue
d = sqrt((obj.position[0] - ps.pose.position.x)**2 +
(obj.position[1] - ps.pose.position.y)**2 +
(obj.position[2] - ps.pose.position.z)**2)
if dist is None or d < dist:
dist = d
c_obj = obj
if c_obj:
self.logdebug("Closest object is: " + c_obj.object_id +
" (dist: " + str(dist) + ")")
else:
self.logdebug("No object of type " + obj_type + " found.")
if c_obj and dist < 0.4:
frame_id = "object_id_" + c_obj.object_id
try:
self.ui.tfl.waitForTransform(frame_id, ps.header.frame_id,
ps.header.stamp,
rospy.Duration(2.0))
ps = self.ui.tfl.transformPose(frame_id, ps)
except tf.Exception:
self.logerr("Failed to transform gripper pose (" +
ps.header.frame_id + ") to object frame_id: " +
frame_id)
return
self.ui.notif(translate(
"DrillPoints",
"Gripper pose relative to object %1 stored").arg(
c_obj.object_id),
temp=True)
self.ui.notify_info()
self.ui.program_vis.update_pose(ps, self.drill_pose_idx)
self.drill_pose_idx += 1
if self.drill_pose_idx >= self.ui.program_vis.get_poses_count():
self.drill_pose_idx = 0
self.drill_dialog.set_caption(self.get_drill_caption())
else:
self.ui.notif(translate("DrillPoints",
"Failed to find object near gripper."),
temp=True,
message_type=NotifyUserRequest.WARN)
self.ui.notify_warn()