def __init__(self, optical_frame, tf_listener=None): if tf_listener == None: tf_listener = tf.TransformListener() self.tf_listener = tf_listener self.optical_frame = optical_frame self.robot = pr2.PR2(self.tf_listener, base=True) self.behaviors = ManipulationBehaviors('l', self.robot, tf_listener=self.tf_listener) self.laser_scan = hd.LaserScanner('point_cloud_srv') self.prosilica = rc.Prosilica('prosilica', 'polled') self.wide_angle_camera_left = rc.ROSCamera('/wide_stereo/left/image_rect_color') self.wide_angle_configure = dr.Client('wide_stereo_both') #TODO: define start location in frame attached to torso instead of base_link self.start_location_light_switch = (np.matrix([0.35, 0.30, 1.1]).T, np.matrix([0., 0., 0., 0.1])) self.start_location_drawer = (np.matrix([0.20, 0.40, .8]).T, np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0))) self.folded_pose = np.matrix([ 0.10134791, -0.29295995, 0.41193769]).T self.driving_param = {'light_switch_up': {'coarse': .9, 'fine': .6, 'voi': .4}, 'light_switch_down': {'coarse': .9, 'fine': .6, 'voi': .4}, 'light_rocker_down': {'coarse': .9, 'fine': .6, 'voi': .4}, 'light_rocker_up': {'coarse': .9, 'fine': .6, 'voi': .4}, 'pull_drawer': {'coarse': .9, 'fine': .5, 'voi': .4}, 'push_drawer': {'coarse': .9, 'fine': .5, 'voi': .4}} self.create_arm_poses() self.driving_posture('light_switch_down') self.robot.projector.set(False)
def __init__(self, object_name, labeled_data_fname, tf_listener): r3d.InterestPointAppBase.__init__(self, object_name, labeled_data_fname) self.tf_listener = tf_listener if tf_listener == None: self.tf_listener = tf.TransformListener() self.laser_scan = hd.LaserScanner('point_cloud_srv') self.prosilica = rc.Prosilica('prosilica', 'polled') self.prosilica_cal = rc.ROSCameraCalibration('/prosilica/camera_info') self.image_pub = r3d.ImagePublisher(object_name + '_perception', self.prosilica_cal) self.last_added = None self.disp = r3d.RvizDisplayThread(self.prosilica_cal) self.disp.start()
if __name__ == '__main__': mode = 'image' #if mode = 'image': # find pose of model # find normal of model # record angles of features. if mode=='image': test_thresholds() if mode=='live': model_name = sys.argv[1] model_img = cv.LoadImage(model_name) model_gray = fea.grayscale(model_img) msurf_loc, msurf_desc = fea.surf(model_gray) prosilica = rc.Prosilica('prosilica', 'streaming') cv.NamedWindow('surf', 1) si = ShowImage('surf') si.start() #Each feature is a row in db features_db = sp.KDTree(np.array(msurf_desc)) #pdb.set_trace() while not rospy.is_shutdown(): print '..' image = prosilica.get_frame() print 'saving image' cv.SaveImage('frame.png', image) print '>' img_gray = fea.grayscale(image)