def __init__(self, *args, **kwargs): super(VisualInspection, self).__init__(*args, **kwargs) self.bridge = CvBridge() topic = "" try: topic = rospy.get_param("/art/visual_inspection/topic") except KeyError: self.logerr("Topic for visual inspection not set!") if not topic: self.logwarn("Using default topic!") topic = "image_color" self.showing_result = False self.to_be_cleaned_up = False try: # TODO maybe this could be in defined in instructions yaml? img_origin = array_from_param("/art/visual_inspection/origin", float, 2) img_size = array_from_param("/art/visual_inspection/size", float, 2) fixed = True except KeyError: img_origin = (0.3, 0.3) img_size = (0.2, 0.1) fixed = False # TODO display image_item after we receive first image? self.img_item = ImageItem(self.ui.scene, img_origin[0], img_origin[1], img_size[0], img_size[1], fixed) self.img_sub = rospy.Subscriber(topic, Image, self.image_callback, queue_size=1) self.result_sub = rospy.Subscriber("/art/visual_inspection/result", Bool, self.result_callback, queue_size=10) self.text_timer = QtCore.QTimer(self) self.text_timer.timeout.connect(self.text_timer_tick) self.text_timer.setSingleShot(True)
def __init__(self, target_frame="marker"): self.target_frame = target_frame self.tfl = tf.TransformListener() self.lock = threading.Lock() self.detection_enabled = True self.use_forearm_cams = False self.table_size = array_from_param("/art/conf/table/size", float, 2, wait=True) self.ground_objects_on_table = rospy.get_param("~ground_objects_on_table", False) self.yaw_only_on_table = rospy.get_param("~yaw_only_on_table", False) self.ground_bb_axis = rospy.get_param("~ground_bb_axis", SolidPrimitive.BOX_Z) if self.ground_objects_on_table: rospy.loginfo("Objects on table will be grounded.") self.api = ArtApiHelper() self.api.wait_for_db_api() self.meas_max_age = rospy.Duration(5.0) self.prune_timer = rospy.Timer(rospy.Duration(1.0), self.prune_timer_cb) self.objects = {} self.br = tf.TransformBroadcaster() self.sub = rospy.Subscriber( "/art/object_detector/object", InstancesArray, self.cb, queue_size=1) self.pub = rospy.Publisher( "/art/object_detector/object_filtered", InstancesArray, queue_size=1, latch=True) self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_cb) self.srv_set_flag = rospy.Service('/art/object_detector/flag/set', ObjectFlagSet, self.srv_set_flag_cb) self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear', ObjectFlagClear, self.srv_clear_flag_cb) self.srv_clear_all_flags = rospy.Service('/art/object_detector/flag/clear_all', Empty, self.srv_clear_all_flags_cb) self.forearm_cams = ("/l_forearm_cam_optical_frame", "/r_forearm_cam_optical_frame") self.srv_enable_forearm = rospy.Service('/art/object_detector/forearm/enable', Empty, self.srv_enable_forearm_cb) self.srv_disable_forearm = rospy.Service('/art/object_detector/forearm/disable', Empty, self.srv_disable_forearm_cb) self.srv_enable_detection = rospy.Service('/art/object_detector/all/enable', Empty, self.srv_enable_detection_cb) self.srv_disable_detection = rospy.Service('/art/object_detector/all/disable', Empty, self.srv_disable_detection_cb)
def __init__(self, setup, world_frame): assert setup != "" self.ready = False self.setup = setup self.world_frame = world_frame self.tf_listener = TransformListener() self.api = ArtApiHelper() rospy.loginfo("Waiting for DB API") self.api.wait_for_db_api() self.ignored_prefixes = array_from_param("~ignored_prefixes") rospy.loginfo("Will ignore following prefixes: " + str(self.ignored_prefixes)) self.lock = RLock() self.ps = PlanningSceneInterface() self._paused = False self.oh = ObjectHelper(self.object_cb) self.artificial_objects = {} self.collision_objects_pub = rospy.Publisher(self.NS + "artificial", CollisionObjects, latch=True, queue_size=1) self.pub_artificial() self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb) self.paused_pub = rospy.Publisher(self.NS + "paused", Bool, latch=True, queue_size=1) self.paused = False
def __init__(self): super(Projector, self).__init__() while True: try: self.world_frame = rospy.get_param('/art/conf/world_frame') break except KeyError: rospy.loginfo("Waiting for global parameters...") rospy.sleep(1.0) self.proj_id = rospy.get_param('~projector_id', 'test') self.screen = rospy.get_param('~screen_number', 0) self.camera_image_topic = rospy.get_param( '~camera_image_topic', 'kinect2/hd/image_color_rect') self.camera_depth_topic = rospy.get_param( '~camera_depth_topic', 'kinect2/hd/image_depth_rect') self.camera_info_topic = rospy.get_param('~camera_info_topic', 'kinect2/hd/camera_info') self.map_x = None self.map_y = None ns = "/art/interface/projected_gui/" self.rpm = int(rospy.get_param(ns + 'rpm')) self.server = rospy.get_param(ns + "scene_server") self.port = rospy.get_param(ns + "scene_server_port") self.scene_size = array_from_param(ns + "scene_size", float, 2) self.scene_origin = array_from_param(ns + "scene_origin", float, 2) rospy.loginfo("Projector '" + self.proj_id + "', on screen " + str(self.screen)) img_path = rospkg.RosPack().get_path('art_projected_gui') + '/imgs' self.checkerboard_img = QtGui.QPixmap(img_path + "/pattern.png") self.calibrating = False self.calibrated = False self.maps_ready = False desktop = QtGui.QDesktopWidget() geometry = desktop.screenGeometry(self.screen) self.move(geometry.left(), geometry.top()) self.resize(geometry.width(), geometry.height()) self.tfl = None self.bridge = CvBridge() self.setAutoFillBackground(True) p = self.palette() p.setColor(self.backgroundRole(), QtCore.Qt.black) self.setPalette(p) self.pix_label = QtGui.QLabel(self) self.pix_label.setAlignment(QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter) # self.pix_label.setScaledContents(True) self.pix_label.resize(self.size()) self.tcpSocket = QtNetwork.QTcpSocket(self) self.blockSize = 0 self.tcpSocket.readyRead.connect(self.getScene) self.tcpSocket.error.connect(self.on_error) self.projectors_calibrated_sub = rospy.Subscriber( '/art/interface/projected_gui/app/projectors_calibrated', Bool, self.projectors_calibrated_cb, queue_size=10) self.projectors_calibrated = False self.srv_calibrate = rospy.Service("~calibrate", Trigger, self.calibrate_srv_cb) self.corners_pub = rospy.Publisher("~corners", PoseArray, queue_size=10, latch=True) QtCore.QObject.connect(self, QtCore.SIGNAL('show_chessboard'), self.show_chessboard_evt) QtCore.QObject.connect(self, QtCore.SIGNAL('show_pix_label'), self.show_pix_label_evt) self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) self.showFullScreen() self.setCursor(QtCore.Qt.BlankCursor) self.calibrated_pub = rospy.Publisher("~calibrated", Bool, queue_size=1, latch=True) h_matrix = rospy.get_param("~calibration_matrix", None) if h_matrix is not None: rospy.loginfo('Loaded calibration from param.') self.calibrated = True self.calibrated_pub.publish(self.is_calibrated()) self.init_map_from_matrix(np.matrix(ast.literal_eval(h_matrix))) else: self.calibrated_pub.publish(self.is_calibrated()) self.connect()
def __init__(self): super(Projector, self).__init__() while not self.kill_now: try: self.world_frame = rospy.get_param('/art/conf/world_frame') break except KeyError: rospy.loginfo("Waiting for global parameters...") rospy.sleep(1.0) if self.kill_now: return self.proj_id = rospy.get_param('~projector_id', 'test') self.screen = rospy.get_param('~screen_number', 0) self.camera_image_topic = rospy.get_param( '~camera_image_topic', 'kinect2/hd/image_color_rect') self.camera_depth_topic = rospy.get_param( '~camera_depth_topic', 'kinect2/hd/image_depth_rect') self.camera_info_topic = rospy.get_param( '~camera_info_topic', 'kinect2/hd/camera_info') # padding serves to restrict area usable for calibration (flat surface) self.padding = Padding() self.map_x = None self.map_y = None self.dx = None self.dy = None self.scaled_checkerboard_width = None self.rpm = int(rospy.get_param(self.ns + 'rpm')) self.scene_size = array_from_param(self.ns + "scene_size", float, 2) self.scene_origin = array_from_param(self.ns + "scene_origin", float, 2) rospy.loginfo("Projector '" + self.proj_id + "', on screen " + str(self.screen)) img_path = rospkg.RosPack().get_path('art_projected_gui') + '/imgs' self.checkerboard_img = QtGui.QPixmap(img_path + "/pattern.png") self.calibrating = False self.calibrated = False self.maps_ready = False desktop = QtGui.QDesktopWidget() geometry = desktop.screenGeometry(self.screen) self.move(geometry.left(), geometry.top()) self.resize(geometry.width(), geometry.height()) self.tfl = None self.bridge = CvBridge() self.setAutoFillBackground(True) p = self.palette() p.setColor(self.backgroundRole(), QtCore.Qt.black) self.setPalette(p) self.projectors_calibrated_sub = rospy.Subscriber( '/art/interface/projected_gui/projectors_calibrated', Bool, self.projectors_calibrated_cb, queue_size=10) self.projectors_calibrated = False self.srv_calibrate = rospy.Service( "~calibrate", Trigger, self.calibrate_srv_cb) self.corners_pub = rospy.Publisher( "~corners", PoseArray, queue_size=10, latch=True) QtCore.QObject.connect(self, QtCore.SIGNAL( 'show_chessboard'), self.show_chessboard_evt) QtCore.QObject.connect(self, QtCore.SIGNAL( 'show_pix_label'), self.show_pix_label_evt) self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) self.showFullScreen() self.setCursor(QtCore.Qt.BlankCursor) self.calibrated_pub = rospy.Publisher( "~calibrated", Bool, queue_size=1, latch=True) h_matrix = rospy.get_param("~calibration_matrix", None) rospack = rospkg.RosPack() self.map_path = os.path.join(rospack.get_path('art_projector'), 'data', self.proj_id + '.ptf') if h_matrix is not None: rospy.loginfo('Loaded calibration from param.') self.calibrated = True self.calibrated_pub.publish(self.is_calibrated()) try: with open(self.map_path, 'rb') as f: self.map_x, self.map_y = pickle.load(f) self.maps_ready = True rospy.loginfo("Map loaded from file") except (IOError, OSError) as e: # TODO pickle exceptions? rospy.logwarn("Failed to load map from file") if not self.maps_ready: self.init_map_from_matrix(np.matrix(ast.literal_eval(h_matrix))) else: try: os.remove(self.map_path) except (IOError, OSError): pass self.calibrated_pub.publish(self.is_calibrated())