def interface_state_cb(self, state):
if state.current_syst_state == InterfaceStateItem.STATE_PROGRAM_RUNNING:
self.program_started = True
self.prog.set_current(state.program_id, state.instruction_id)
# TODO other states!!!
#elif state.current_syst_state == InterfaceStateItem.STATE_LEARNING:
# pass
if state.is_clear(): self.clear_all_evt()
# selected object IDs
for obj_id, obj in self.viz_objects.iteritems():
if obj_id in state.selected_object_ids:
obj.set_selected()
elif obj.obj_type not in state.selected_object_types:
obj.unselect()
# TODO selected object types
for obj_id, obj in self.viz_objects.iteritems():
if obj.obj_type in state.selected_object_types:
obj.set_selected()
elif obj_id not in state.selected_object_ids:
obj.unselect()
# TODO check if polygon already exists
# TODO remove polygons
for poly in state.polygons:
self.viz_polygons.append(scene_polygon(self.scene, self.calib, poly.polygon.points))
def pointing_point(self, pt, click = False):
if self.selected_at is not None:
if rospy.Time.now() - self.selected_at < rospy.Duration(3):
pt.disable()
return
self.selected_at = None
if self.program_started:
return
if self.user_status is None or self.user_status.user_state != UserStatus.USER_CALIBRATED:
return
if self.item_to_learn is None:
self.item_to_learn = self.prog.get_item_to_learn()
if self.item_to_learn is not None:
self.state_manager.set_syst_state(InterfaceStateItem.STATE_LEARNING, self.prog.prog.id, self.item_to_learn.id)
self.object_selected = False
self.place_selected = False
self.polygon_selected = False
# everything is already learned - let's start program execution
if self.item_to_learn is None:
self.program_started = True
self.start_program()
return
if self.item_to_learn.type == ProgramItem.MANIP_PICK_PLACE:
if self.place_selected:
self.item_to_learn = None
self.emit(QtCore.SIGNAL('clear_all()'))
return
if not self.object_selected:
self.select_object(pt, click)
return
if self.item_to_learn.spec == ProgramItem.MANIP_TYPE:
if self.polygon_selected is False:
if len(self.viz_polygons) == 0: self.viz_polygons.append(scene_polygon(self.scene, self.calib, []))
self.select_polygon(pt, click)
return
sp = self.select_place(pt, click)
if self.place_selected:
self.item_to_learn.place_pose = sp.get_pose()
self.item_to_learn.pick_polygon = self.viz_polygons[0].msg()
self.label.setPlainText("Step learned")
self.prog.learned(self.item_to_learn.id)
self.store_program()
else:
# TODO other types
pass
