def __init__(self, *args, **kwargs):
super(PickFromPolygonRun, self).__init__(*args, **kwargs)
obj_id = None
try:
obj_id = self.flags["SELECTED_OBJECT_ID"]
except KeyError:
self.logerr("SELECTED_OBJECT_ID flag not set")
if obj_id is not None:
self.ui.select_object(obj_id)
obj = self.ui.get_object(obj_id) # TODO notif - object type
if obj is not None:
self.ui.notif(
translate("PickFromPolygon", "Going to pick object ID ") +
obj_id + translate("PickFromPolygon", " of type ") +
obj.object_type.name +
translate("PickFromPolygon", " from polygon."))
self.ui.add_polygon(translate("PickFromPolygon", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(
self.ui.ph.get_polygon(
self.block_id, self.instruction_id)[0]),
fixed=True)
def __init__(self, *args, **kwargs):
super(PlaceToGridVis, self).__init__(*args, **kwargs)
self.ui.select_object_type(self.ph.get_object(*self.cid)[0][0])
polygons = self.ui.ph.get_polygon(*self.cid)[0]
poses = self.ui.ph.get_pose(*self.cid)[0]
object_type_name = self.ui.ph.get_object(*self.cid)[0][0]
object_type = self.ui.art.get_object_type(object_type_name)
self.ui.notif(
translate("PlaceToGrid", "Going to place objects into grid"))
self.ui.add_square(
translate("PlaceToGrid", "PLACE SQUARE GRID"),
self.ui.width / 2,
self.ui.height / 2,
0.1,
0.075,
object_type,
poses,
grid_points=conversions.get_pick_polygon_points(polygons),
square_changed=self.ui.square_changed,
fixed=True)
def __init__(self, *args, **kwargs):
super(PlaceToContainerLearn, self).__init__(*args, **kwargs)
# TODO check if pick is set?
if not self.ui.ph.is_object_set(*self.cid):
if self.editable:
self.ui.notif(
translate("PlaceToContainer", "Select container by tapping on its outline."))
return
object_type_name = self.ui.ph.get_object(*self.cid)[0][0]
self.ui.select_object_type(object_type_name)
if self.ui.ph.is_polygon_set(*self.cid):
polygons = self.ui.ph.get_polygon(*self.cid)[0]
self.ui.add_polygon(
translate(
"PlaceToContainer",
"Containers"),
poly_points=conversions.get_pick_polygon_points(polygons),
polygon_changed=self.ui.polygon_changed,
fixed=not self.editable)
if self.editable:
self.ui.notif(
translate("PlaceToContainer", "Adjust area with containers or select another object type."))
def __init__(self, *args, **kwargs):
super(PickFromPolygonLearn, self).__init__(*args, **kwargs)
if not self.ui.ph.is_object_set(*self.cid):
if self.editable:
self.ui.notif(
translate(
"PickFromPolygon",
"Select object type to be picked up by tapping on its outline."
))
else:
object_type_name = self.ui.ph.get_object(*self.cid)[0][0]
self.ui.select_object_type(object_type_name)
if self.ui.ph.is_polygon_set(*self.cid):
polygons = self.ui.ph.get_polygon(*self.cid)[0]
self.ui.add_polygon(
translate("PickFromPolygon", "PICK AREA"),
poly_points=conversions.get_pick_polygon_points(polygons),
polygon_changed=self.ui.polygon_changed,
fixed=not self.editable)
if self.editable:
self.ui.notif(
translate(
"PickFromPolygon",
"Adjust pick area or select another object type."))
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 __init__(self, *args, **kwargs):
super(PickFromPolygonVis, self).__init__(*args, **kwargs)
self.ui.select_object_type(self.ui.ph.get_object(*self.cid)[0][0])
self.ui.add_polygon(translate("PickFromPolygon", "PICK POLYGON"),
poly_points=conversions.get_pick_polygon_points(
self.ui.ph.get_polygon(*self.cid)[0]),
fixed=True)
def __init__(self, *args, **kwargs):
super(DrillPointsVis, self).__init__(*args, **kwargs)
self.ui.select_object_type(self.ui.ph.get_object(*self.cid)[0][0])
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),
fixed=True)
def __init__(self, *args, **kwargs):
super(DrillPointsRun, self).__init__(*args, **kwargs)
polygons = self.ui.ph.get_polygon(*self.cid)[0]
poses = self.ui.ph.get_pose(*self.cid)[0]
try:
self.ui.select_object(self.flags["SELECTED_OBJECT_ID"])
self.ui.notif(
translate(
"DrillPoints",
"Going to drill hole %1 out of %2 into object %3.").arg(
self.flags["DRILLED_HOLE_NUMBER"]).arg(len(poses)).arg(
self.flags["SELECTED_OBJECT_ID"]))
except KeyError as e:
self.logerr("flag not set: " + str(e))
self.ui.add_polygon(
translate("DrillPoints", "Objects to be drilled"),
poly_points=conversions.get_pick_polygon_points(polygons),
fixed=True)
def __init__(self, *args, **kwargs):
super(PlaceToContainerRun, self).__init__(*args, **kwargs)
obj_id = None
try:
obj_id = self.flags["SELECTED_OBJECT_ID"]
except KeyError:
self.logerr("SELECTED_OBJECT_ID flag not set")
cont_id = None
try:
cont_id = self.flags["SELECTED_CONTAINER_ID"]
except KeyError:
self.logerr("SELECTED_CONTAINER_ID flag not set")
if cont_id:
self.ui.select_object(cont_id)
if obj_id:
self.ui.notif(translate("PlaceToContainer", "Placing %1 to %2...").arg(obj_id).arg(cont_id))
else:
self.ui.notif(translate("PlaceToContainer", "Placing to %1...").arg(cont_id))
try:
show_area = self.ui.ih["PlaceToContainer"].gui.params["show_area_run"]
except KeyError:
show_area = True
if show_area:
self.ui.add_polygon(
translate(
"PlaceToContainer",
"Containers"),
poly_points=conversions.get_pick_polygon_points(
self.ui.ph.get_polygon(*self.cid)[0]),
fixed=True)
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 state_running(self, old_state, state, flags, system_state_changed):
if system_state_changed:
if not self.ph.load(self.art.load_program(state.program_id)):
self.notif(
translate("UICoreRos",
"Failed to load program from database."))
# TODO what to do?
return
stopped = state.system_state == InterfaceState.STATE_PROGRAM_STOPPED
self.show_program_vis(readonly=True, stopped=stopped)
if stopped:
self.notif(translate("UICoreRos", "Program paused."),
temp=True)
if not stopped and old_state.system_state == InterfaceState.STATE_PROGRAM_STOPPED:
self.notif(translate("UICoreRos", "Program resumed."),
temp=True)
# ignore not valid states
if state.block_id == 0 or state.program_current_item.id == 0:
rospy.logerr("Invalid state!")
return
# TODO if the item id is same - do rather update then clear + add everything?
self.clear_all()
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
if obj is not None:
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)
elif it.type == ProgIt.PLACE_TO_GRID:
ref_msg = self.program_vis.get_ref_item(
it.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", "Going to place object into grid"))
self.add_square(
translate("UICoreRos", "PLACE SQUARE GRID"),
self.width / 2,
self.height / 2,
0.1,
0.075,
object_type,
it.pose,
grid_points=conversions.get_pick_polygon_points(it),
square_changed=self.square_changed,
fixed=True)
def active_item_switched(self, block_id, item_id, read_only=True):
rospy.logdebug("Program ID:" + str(self.ph.get_program_id()) +
", active item ID: " + str((block_id, item_id)))
self.clear_all()
if item_id is None:
# TODO hlaska
return
self.state_manager.update_program_item(
self.ph.get_program_id(), block_id,
self.ph.get_item_msg(block_id, item_id))
if not self.ph.item_requires_learning(block_id, item_id):
return
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.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)
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
