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())