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)