def __init__(self): self.show_marker_service = rospy.get_param('show_marker_service', '/art/interface/projected_gui/show_marker') self.hide_marker_service = rospy.get_param('hide_marker_service', '/art/interface/projected_gui/hide_marker') self.table_localize_action = rospy.get_param('table_localize_action', '/umf_localizer_node_table/localize') self.pr2_localize_action = rospy.get_param('pr2_localize_action', '/umf_localizer_node/localize') self.calibrate_pr2 = rospy.get_param('calibrate_pr2', False) self.calibrate_table = rospy.get_param('calibrate_table', False) self.user_status_sub = rospy.Subscriber("/art/user/status", UserStatus, self.user_status_cb) self.user_activity_sub = rospy.Subscriber("/art/user/activity", UserActivity, self.user_activity_cb) self.srv_program_start = rospy.Service('/art/brain/program/start', startProgram, self.program_start_cb) self.srv_program_stop = rospy.Service('/art/brain/program/stop', Empty, self.program_stop_cb) #self.srv_program_pause = rospy.Service(/art/brain/program/pause', Empty, self.program_pause_cb) #self.srv_program_resume = rospy.Service(/art/brain/program/resume', Empty, self.program_resume_cb) self.state_manager = interface_state_manager(InterfaceState.BRAIN_ID) self.user_activity = None self.objects_sub = rospy.Subscriber("/art/object_detector/object_filtered", InstancesArray, self.objects_cb) self.state_publisher = rospy.Publisher("/art/brain/system_state", SystemState, queue_size=1) self.pp_client = actionlib.SimpleActionClient('/art/pr2/left_arm/pp', pickplaceAction) self.state = self.SYSTEM_START self.user_id = 0 self.objects = InstancesArray() self.executing_program = False self.instruction = None self.holding_object = None self.stop_server = False self.recalibrate = False self.quit = False self.program = None self.prog_id = None self.it_id = None
def __init__(self): super(simple_gui, self).__init__() self.tfl = None self.inited = False rospack = rospkg.RosPack() self.img_path = rospack.get_path('art_projected_gui') + '/imgs' self.obj_sub = rospy.Subscriber('/art/object_detector/object_filtered', InstancesArray, self.object_cb, queue_size=1) self.point_left_sub = rospy.Subscriber('/art/user/pointing_left', PoseStamped, self.pointing_point_left_cb, queue_size=1) self.point_right_sub = rospy.Subscriber('/art/user/pointing_right', PoseStamped, self.pointing_point_right_cb, queue_size=1) self.user_status_sub = rospy.Subscriber('/art/user/status', UserStatus, self.user_status_cb, queue_size=1) self.enabled_int = True self.srv_enable_int = rospy.Service('~enable', Empty, self.enable_int) self.srv_disable_int = rospy.Service('~disable', Empty, self.disable_int) self.srv_show_marker = rospy.Service('~show_marker', Empty, self.show_marker) self.srv_hide_marker = rospy.Service('~hide_marker', Empty, self.hide_marker) self.viz_objects = {} # objects can be accessed by their ID self.viz_places = [] # array of scene_place objects self.viz_polygons = [] # array of scene_polygon objects # these are used to know progress of task item programming self.object_selected = False self.place_selected = False self.polygon_selected = False self.selected_at = None # timestamp of last learned program item self.scene=QtGui.QGraphicsScene(self) self.scene.setBackgroundBrush(QtCore.Qt.black) self.view = QtGui.QGraphicsView(self.scene, self) self.view.setRenderHint(QtGui.QPainter.Antialiasing) self.view.setViewportUpdateMode(QtGui.QGraphicsView.FullViewportUpdate) self.view.setStyleSheet( "QGraphicsView { border-style: none; }" ) #self.view.setViewport(QtOpenGL.QGLWidget()) # rendering using OpenGL -> somehow broken :( self.pm = QtGui.QPixmap(self.img_path + "/koberec.png") # TODO use homography matrix to correct it self.marker = self.scene.addPixmap(self.pm.scaled(self.size(), QtCore.Qt.KeepAspectRatio)) self.marker.setZValue(-100) self.marker.hide() self.resizeEvent = self.on_resize self.pointing_left = pointing_point("left", self.scene) self.pointing_right = pointing_point("right", self.scene) self.pointing_mouse = pointing_point("mouse", self.scene, True) QtCore.QObject.connect(self, QtCore.SIGNAL('objects'), self.objects_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('pointing_point_left'), self.pointing_point_left_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('pointing_point_right'), self.pointing_point_right_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('clear_all()'), self.clear_all_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('show_marker()'), self.show_marker_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('hide_marker()'), self.hide_marker_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('user_status'), self.user_status_evt) self.timer = QtCore.QTimer() self.timer.start(500) self.timer.timeout.connect(self.timer_evt) self.label = self.scene.addText("Waiting for user", QtGui.QFont('Arial', 26)) self.label.rotate(180) self.label.setDefaultTextColor(QtCore.Qt.white) self.label.setZValue(200) self.user_status = None self.calib = gui_calibration(self.scene, self.img_path, self.width()) self.calib.on_request = self.on_calib_req self.calib.on_finished = self.on_calib_finished self.ignored_items = [self.label, self.marker, self.calib.checkerboard] # items in the scene to be ignored when checking for "collisions" self.prog = program_widget(self) self.prog.resize(500, 200) self.prog.move(10, 10) self.prog.show() self.srv_brain_start_program = rospy.ServiceProxy('/art/brain/program/start', startProgram) # TODO only for testing - program should be selected by user self.load_program(0) # we will save learned program under different ID self.prog.prog.id = 1 # TODO don't have it hardcoded self.program_started = False self.item_to_learn = None self.state_manager = interface_state_manager("PROJECTED UI", cb=self.interface_state_cb) self.state_manager.int_state(False) self.inited = True