コード例 #1
0
def save_clouds():
    NUM_CAM      = 2   
    cameras      = RosCameras(NUM_CAM)
    tfm_listener = tf.TransformListener()
    fr1 = camera1_rgb_optical_frame
    fr2 = camera2_rgb_optical_frame
    c1, c2 = 0, 1

    print "Waiting for service .."
    rospy.wait_for_service('pcd_service')
    pcdService = rospy.ServiceProxy("pcd_service", PCDData)
    
    raw_input(colorize("Do not move the objects on the table now.", "green", True))

    raw_input(colorize("Cover camera %i and hit enter!" % (c2 + 1), 'yellow', True))
    pc1 = cameras.get_pointcloud(c1)
    raw_input(colorize("Cover camera %i and hit enter!" % (c1 + 1), 'yellow', True))
    pc2 = cameras.get_pointcloud(c2)
    pc2 = ru.transformPointCloud2(pc2, tfm_listener, fr1, fr2)
    req = PCDDataRequest()

    req.pc = pc1
    req.fname = "cloud1.pcd"
    pcdService(req)

    req.pc = pc2
    req.fname = "cloud2.pcd"
    pcdService(req)
コード例 #2
0
    def process_request(self, req):
        if req['type'] == 'custom':
            try:
                f = self.plotting_funcs[req['func']]        
            except KeyError:
                print colorize("No custom plotting function with name : %s. Ignoring plot request."%req['func'], "red")

        elif req['type'] == 'mlab':
            try:
                f = getattr(mlab, req['func'])
            except KeyError:
                print colorize("No mlab plotting function with name : %s. Ignoring plot request."%req['func'], "red")
        
        args, kwargs = req['data']
        f(*args, **kwargs)
コード例 #3
0
    def calibrate (self, method='ar', use_icp=False, n_obs=10, n_avg=5):
        print method
        if self.num_cameras == 1:
            redprint ("Only one camera. You don't need to calibrate.", True)
            self.calibrated = True
            self.cameras.calibrated = True
            return
        
        self.initialize_calibration(method)
        n_effective_obs = 0
        while n_effective_obs < n_obs:
            yellowprint("Please hold still for a few seconds. Make sure the transforms look good on rviz.")
            raw_input(colorize("Observation %d from %d. Press return when ready." % (n_effective_obs+1, n_obs), 'green', True))
            if method == 'ar':
                calibration_updated = self.process_observation_ar(n_avg)
            elif method == 'cb':
                calibration_updated = self.process_observation_cb()
            else:
                calibration_updated = self.process_observation_pycb()
            if calibration_updated: n_effective_obs += 1

        if method == 'pycb':
            self.calibrated = self.finish_calibration_pycb()
        else:
            self.calibrated = self.finish_calibration(use_icp)
            
        self.cameras.calibrated = self.calibrated
コード例 #4
0
def get_first_state(dt, c1_ts, c1_tfs, c2_ts, c2_tfs, hy_ts, hy_tfs):
    """
    Return the first state (mean, covar) to initialize the kalman filter with.
    Assumes that the time-stamps start at a common zero (are on the same time-scale).
    
    Returns a state b/w t=[0, dt]
    
    Gives priority to AR-markers. If no ar-markers are found in [0,dt], it returns
    hydra's estimate but with larger covariance.
    """
    
    ar1 = [c1_tfs[i] for i in xrange(len(c1_ts)) if c1_ts[i] <= dt]
    ar2 = [c2_tfs[i] for i in xrange(len(c2_ts)) if c2_ts[i] <= dt]
    hy =  [hy_tfs[i] for i in xrange(len(hy_ts)) if hy_ts[i] <= dt] 
    
    if ar1 != [] or ar2 != []:
        ar1.extend(ar2)
        x0 =  state_from_tfms_no_velocity([avg_transform(ar1)])
        I3 = np.eye(3)
        S0 = scl.block_diag(1e-3*I3, 1e-2*I3, 1e-3*I3, 1e-3*I3)
    else:
        assert len(hy)!=0, colorize("No transforms found for KF initialization. Aborting.", "red", True)
        x0 = state_from_tfms_no_velocity([avg_transform(hy)])
        I3 = np.eye(3)
        S0 = scl.block_diag(1e-1*I3, 1e-1*I3, 1e-2*I3, 1e-2*I3)
    return (x0, S0)
コード例 #5
0
    def process_observation(self, n_avg=5):
        """
        Store an observation of ar_marker and hydras.
        """
        
        raw_input(colorize("Press return when ready to capture transforms.", "green", True))
        
        ar_avg_tfm = []
        hydra_avg_tfm = []
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)

            ar_transforms = gmt.get_ar_markers_from_cameras(self.cameras, cams = [self.calib_camera], markers = [self.ar_marker])
            hydra_transforms = gmt.get_hydra_transforms('hydra_base', [self.calib_hydra])
            ar_avg_tfm.append(ar_transforms[self.ar_marker])
            hydra_avg_tfm.append(hydra_transforms[self.ar_marker][self.calib_hydra])
            
        self.ar_transforms.append(utils.avg_transform(ar_avg_tfm))
        self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
コード例 #6
0
    def get_tfm_from_obs(self, hydra_marker, n_tfm=15, n_avg=30):
        """
        Stores a bunch of readings from sensors.
        """
        all_tfms = []
        
        if rospy.get_name == '/unnamed':
            rospy.init_node('gripper_calib')
        sleeper = rospy.Rate(30)
    
        i = 0
        n_attempts_max = n_avg*2
        while i < n_tfm:
            raw_input(colorize("Getting transform %i out of %i. Hit return when ready."%(i, n_tfm-1), 'yellow', True))

            j = 0
            n_attempts_effective = 0;
            hyd_tfms = []
            ar_tfms = []
            while j < n_attempts_max:
                j += 1
                ar_tfms_j = self.cameras.get_ar_markers(markers=[self.ar_marker]);
                hyd_tfms_j = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=[hydra_marker]);
                
                if not ar_tfms_j or not hyd_tfms_j:
                    if not ar_tfms_j:
                        yellowprint("Could not find required ar markers from " + str(self.ar_marker))
                    else:
                        yellowprint("Could not find required hydra transforms from " + str(hydra_marker))
                    continue
                                
                ar_tfms.append(ar_tfms_j[self.ar_marker])
                hyd_tfms.append(hyd_tfms_j[hydra_marker])
                
                blueprint('\tGetting averaging transform : %d of %d ...'%(n_attempts_effective, n_avg-1))
                n_attempts_effective += 1 
                
                if n_attempts_effective == n_avg:
                    break
                
                sleeper.sleep()
            
            if n_attempts_effective < n_avg:
                yellowprint("Not enough transforms were collected; try again")
                continue

            ar_tfm = avg_transform(ar_tfms)
            hyd_tfm = avg_transform(hyd_tfms)

            all_tfms.append(nlg.inv(ar_tfm).dot(hyd_tfm))
            i += 1
            
        return avg_transform(all_tfms)
コード例 #7
0
def extract_snapshots_on_cloud(demo_type, core_type):
    """
    runs snapshot extraction on the cloud and saves the result on local machine. 
    """
    demo_testing_dir = osp.join(testing_results_dir, demo_type)
    env_state_files  = find_recursive(demo_testing_dir, '*.cp')
    
    state_infos = []
    for env_state_file in env_state_files[0:2]:
        with open(env_state_file,"r") as fh:
            seg_info = cp.load(fh)['seg_info']
    
        if seg_info == None:
            continue

        save_dir = osp.join(osp.dirname(env_state_file),  'snapshots', osp.splitext(osp.basename(env_state_file))[0])        
        state_infos.append((seg_info, save_dir))

    print colorize("calling on cloud..", "yellow", True)
    jids = cloud.map(get_state_snapshots, state_infos, _env='RSS3', _type=core_type)
    res  = cloud.result(jids)
    print colorize("got snapshots from cloud for : %s. Saving..."%demo_type, "green", True)
    save_snapshots_from_cloud(res)
コード例 #8
0
    def process_observation(self, n_avg=5):
        """
        Store an observation of ar_marker and hydras.
        """        
        raw_input(colorize("Press return when ready to capture transforms.", "green", True))
        
        calib_avg_tfm = []
        hydra_avg_tfm = []
        j = 0
        thresh = n_avg*2
        
        sleeper = rospy.Rate(30)
        while j < n_avg:
            print colorize('\tGetting averaging transform : %d of %d ...'%(j+1,n_avg), "blue", True)

            calib_tfm = self.calib_func()
            hydra_tfm = self.get_hydra_transform()
            if calib_tfm is None or hydra_tfm is None:
                if calib_tfm is None:
                    yellowprint('Could not find all required AR transforms.')
                else:
                    yellowprint('Could not find all required hydra transforms.')
                thresh -= 1
                if thresh == 0:
                    redprint('Tried too many times but could not get enough transforms.')
                    return False
                
                continue
                
            calib_avg_tfm.append(calib_tfm)
            hydra_avg_tfm.append(hydra_tfm)
            j += 1
            sleeper.sleep()

        self.calib_transforms.append(utils.avg_transform(calib_avg_tfm))
        self.hydra_transforms.append(utils.avg_transform(hydra_avg_tfm))
        return True
コード例 #9
0
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        sleeper = rospy.Rate(30)
        avg_tfms = {}
        j = 0
        thresh = n_avg*2
        pot_avg = 0.0
        while j < n_avg:
            blueprint('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1))    

            tfms = {}
            # F**k the frames bullshit
            ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers)
            hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
            pot_angle = gmt.get_pot_angle()
            
            if not ar_tfm or (not hyd_tfm and self.hydras):
                yellowprint('Could not find all required transforms.')
                thresh -= 1
                if thresh == 0: return False
                continue
            
            pot_avg += pot_angle
            
            j += 1
            tfms.update(ar_tfm)
            tfms.update(hyd_tfm)

            # The angle relevant for each finger is only half the angle.

                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
                
            sleeper.sleep()
            
        pot_avg /= n_avg
        
        greenprint("Average angle found: %f"%pot_avg)

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
        return True
コード例 #10
0
    def process_observation(self, n_avg=5):
        """
        Get an observation and update transform list.
        """
        
        yellowprint("Please hold still for a few seconds.")

        self.observation_info = {i:[] for i in xrange(self.num_cameras)}
        self.observed_ar_transforms = {i:{} for i in xrange(self.num_cameras)}

        # Get RGBD observations
        for i in xrange(n_avg):
            print colorize("Transform %d out of %d for averaging."%(i,n_avg),'yellow',False)
            data = self.cameras.get_RGBD()
            for j,cam_data in data.items():
                self.observation_info[j].append(cam_data)
        
        # Find AR transforms from RGBD observations and average out transforms.
        for i in self.observation_info:
            for obs in self.observation_info[i]:
                ar_pos = gmt.get_ar_marker_poses (obs['rgb'], obs['depth'])
                for marker in ar_pos:
                    if self.observed_ar_transforms[i].get(marker) is None:
                        self.observed_ar_transforms[i][marker] = []
                    self.observed_ar_transforms[i][marker].append(ar_pos[marker])
            for marker in self.observed_ar_transforms[i]:
                self.observed_ar_transforms[i][marker] = utils.avg_transform(self.observed_ar_transforms[i][marker]) 

        for i in xrange(1,self.num_cameras):
            transform = self.find_transform_between_cameras_from_obs(0, i)
            if transform is None:
                redprint("Did not find a transform between cameras 0 and %d"%i)
                continue
            if self.transform_list.get(0,i) is None:
               self.transform_list[0,i] = []
            self.transform_list[0,i].append(transform)
コード例 #11
0
 def calibrate (self, n_obs=10, n_avg=5, tfm_pub=None):
     if self.num_cameras == 1:
         redprint ("Only one camera. You don't need to calibrate.", True)
         self.calibrated = True
         self.cameras.set_calibrated(True)
         return
     
     self.initialize_calibration()
     for i in range(n_obs):
         yellowprint ("Transform %d out of %d."%(i,n_obs))
         if tfm_pub is not None: tfm_pub.set_publish_pc(True)
         raw_input(colorize("Press return when you're ready to take the next observation from the cameras.",'green',True))
         if tfm_pub is not None: tfm_pub.set_publish_pc(False)
         self.process_observation(n_avg)
     self.calibrated = self.finish_calibration()
     self.cameras.set_calibrated(self.calibrated)
コード例 #12
0
def calibrate_gripper_lite(lr):
    global cameras, tfm_pub

    greenprint('Step 3.1 Calibrate %s gripper.'%lr)
        
    gr = gripper_lite.GripperLite(lr, marker=gripper_marker_id[lr], 
                                  cameras=cameras,
                                  trans_marker_tooltip=gripper_trans_marker_tooltip[lr])     
        
    tfm_pub.add_gripper(gr)
    tfm_pub.set_publish_grippers(True)

    lr_long = {'l':'left','r':'right'}[lr]

    if yes_or_no(colorize('Do you want to add hydra to %sgripper?'%lr, 'yellow')):
        gr.add_hydra(hydra_marker=lr_long, tfm=None, 
                     ntfm=GRIPPER_LITE_N_OBS, 
                     navg=GRIPPER_LITE_N_AVG)
コード例 #13
0
    def get_obs_new_marker(self, marker, n_tfm=10, n_avg=10):
        """
        Store a bunch of readings seen for new marker being added.
        Returns a list of dicts each of which has marker transforms found
        and potentiometer angles.
        """
        
        all_obs = []
        
        i = 0
        thresh = n_avg * 2
        sleeper = rospy.Rate(30)
        
        while i < n_tfm:
            raw_input(colorize("Getting transform %i out of %i. Hit return when ready." % (i, n_tfm), 'yellow', True))
            
            j = 0
            j_th = 0
            pot_angle = 0
            found = True
            avg_tfms = []
            while j < n_avg:
                blueprint("Averaging transform %i out of %i." % (j, n_avg))
                ar_tfms = self.cameras.get_ar_markers();
                hyd_tfms = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=None);
                curr_angle = gmt.get_pot_angle(self.lr)
                
                if not ar_tfms and not hyd_tfms:
                    yellowprint("Could not find any transform.")
                    j_th += 1
                    if j_th < thresh:
                        continue
                    else:
                        found = False
                        break
                                
                tfms = ar_tfms
                pot_angle += curr_angle
                tfms.update(hyd_tfms)

                avg_tfms.append(tfms)
                j += 1
                sleeper.sleep()
            
            if found is False:
                yellowprint("Something went wrong; try again.")
                continue
            
            tfms_found = {}
            for tfms in avg_tfms:
                for m in tfms:
                    if m not in tfms_found:
                        tfms_found[m] = []
                    tfms_found[m].append(tfms[m])

            if marker not in tfms_found:
                yellowprint("Could not find marker to be added; try again.")
                continue
            
            for m in tfms_found:
                tfms_found[m] = utils.avg_transform(tfms_found[m])
            pot_angle = pot_angle / n_avg
            
            all_obs.append({'tfms':tfms_found, 'pot_angle':pot_angle})
            i += 1
            
        return all_obs
コード例 #14
0
def initialize_simple (n_avg=30):
    """
    Initializes the file to load from for simple feedback.
    
    Creates AABB out of specified points.
    """
    
    if rospy.get_name() == '/unnamed':
        rospy.init_node('simple_feedback_init', anonymous=True)
    sleeper = rospy.Rate(30)
    
    yellowprint("You will be asked to move gripper around the feasibility zone.")
    
    lr = raw_input(colorize("Please enter l/r based on which gripper you are using for initialization: ",'yellow',True))
    lr_long = {'l':'left', 'r':'right'}[lr]


    aabb_points = []

    raw_input(colorize("Place the gripper at bottom-left of reachable region closer to you.",'blue',True))
    attempts = 10
    while attempts > 0:
        try:
            blc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
            print blc_tfm
            aabb_points.append(blc_tfm[0:3,3])
            break
        except Exception as e:
            print e
            attempts -= 1
            yellowprint("Failed attempt. Trying %i more times."%attempts)
            sleeper.sleep()
    if attempts == 0:
        redprint("Could not find hydra.")
    
    raw_input(colorize("Place the gripper at bottom-right of reachable region closer to you.",'blue',True))
    attempts = 10
    while attempts > 0:
        try:
            brc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
            print brc_tfm[0:3,3]
            aabb_points.append(brc_tfm[0:3,3])
            break
        except:
            attempts -= 1
            yellowprint("Failed attempt. Trying %i more times."%attempts)
            sleeper.sleep()
    if attempts == 0:
        redprint("Could not find hydra.")
        
    raw_input(colorize("Place the gripper at top-left of reachable region closer to you.",'blue',True))
    attempts = 10
    while attempts > 0:
        try:
            tlc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
            print tlc_tfm[0:3,3]
            aabb_points.append(tlc_tfm[0:3,3])
            break
        except:
            attempts -= 1
            yellowprint("Failed attempt. Trying %i more times."%attempts)
            sleeper.sleep()
    if attempts == 0:
        redprint("Could not find hydra.")
    
    raw_input(colorize("Place the gripper at top-right of reachable region closer to you.",'blue',True))
    attempts = 10
    while attempts > 0:
        try:
            trc_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
            print trc_tfm[0:3,3]
            aabb_points.append(trc_tfm[0:3,3])
            break
        except:
            attempts -= 1
            yellowprint("Failed attempt. Trying %i more times."%attempts)
            sleeper.sleep()
    if attempts == 0:
        redprint("Could not find hydra.")
         
    raw_input(colorize("Place the gripper as far from you as possible in the reachable region.",'blue',True))
    attempts = 10
    while attempts > 0:
        try:
            far_tfm = get_avg_hydra_tfm(lr_long, n_avg, sleeper)[lr_long]
            print far_tfm[0:3,3]
            aabb_points.append(far_tfm[0:3,3])
            break
        except:
            attempts -= 1
            yellowprint("Failed attempt. Trying %i more times."%attempts)
            sleeper.sleep()
    if attempts == 0:
        redprint("Could not find hydra.")
    
    buffer = 0.05 # a little buffer for reachability
    aabb_points = np.asarray(aabb_points)
    mins = aabb_points.min(axis=0) - buffer
    maxes = aabb_points.max(axis=0) + buffer
    aabb_data = {'mins':mins, 'maxes':maxes}

    yellowprint("Saving AABB in file...")
    aabb_file = osp.join(feedback_dir, simple_feedback_name)
    with open(aabb_file,'w') as fh: cPickle.dump(aabb_data, fh)
    yellowprint("Saved!")
コード例 #15
0
def initialize_ik (calib_file, n_tfm=5, n_avg=30):
    """
    Initializes the file to load from for ik feedback.
    
    First calibrates between PR2 and workspace camera.
    Finally ends up with a calibration from the PR2 base with the hydra_base.
    Can use hydra_info to reach tool tip from movement data. 
    """
    
    if rospy.get_name() == '/unnamed':
        rospy.init_node('ik_feedback_init')
    
    cameras = RosCameras()
    tfm_pub = CalibratedTransformPublisher(cameras)

    yellowprint("Start up the overhead camera as camera1 and PR2's camera as camera2.")
    raw_input(colorize("Hit enter when the PR2 is ready at the workstation.",'yellow',True))

    c1_frame = 'camera1_link'
    h_frame = 'hydra_base'

    # Find tfm from bf to camera1_link    
    greenprint("Calibrating cameras.")
    cam_calib = CameraCalibrator(cameras)
    done = False
    while not done:
        cam_calib.calibrate(n_obs=n_tfm, n_avg=n_avg)
        if not cam_calib.calibrated:
            redprint("Camera calibration failed.")
            cam_calib.reset_calibration()
        else:
            # Add camera to camera tfm and camera to PR2 tfm
            tfm_cam = cam_calib.get_transforms()[0]
            tfm_pr2 = {'parent':'base_footprint', 'child':c1_frame}
            tfm_bf_c2l = get_transform('base_footprint','camera_link')
            tfm_pr2['tfm'] = tfm_bf_c2l.dot(np.linalg.inv(tfm_cam['tfm']))

#             tfm_pub.add_transforms(cam_calib.get_transforms())
            tfm_pub.add_transforms([tfm_cam, tfm_pr2])
            print tfm_pub.transforms
            
            if yes_or_no("Are you happy with the calibration? Check RVIZ."):
                done = True
                tfm_pub.save_calibration(cam_pr2_calib_name)
                greenprint("Cameras calibrated.")
            else:
                yellowprint("Calibrating cameras again.")
                cam_calib.reset_calibration()        

    tfm_c1_h = None
    # Now, find transform between base_footprint and hydra base
    with open(calib_file,'r') as fh: calib_data = cPickle.load(fh)
    for tfm in calib_data['transforms']:
        if tfm['parent'] == c1_frame or tfm['parent'] == '/' + c1_frame:
            if tfm['child'] == h_frame or tfm['child'] == '/' + h_frame:
                tfm_c1_h = tfm['tfm']

    if tfm_c1_h is None:
        redprint("Hydra calib info not found in %s."%calib_file)
    
    grippers = calib_data['grippers']
    assert 'l' in grippers.keys() and 'r' in grippers.keys(), 'Calibration does not have both grippers.'
    
    gprs = {}
    for lr,gdata in grippers.items():
        gr = gripper_lite.GripperLite(lr, gdata['ar'], trans_marker_tooltip=gripper_trans_marker_tooltip[lr])
        gr.reset_gripper(lr, gdata['tfms'], gdata['ar'], gdata['hydra'])
        gprs[lr] = gr
    
    ik_file_data = {}
    ik_file_data['pr2'] = tfm_pr2['tfm'].dot(tfm_c1_h)
    lr_long = {'l':'left','r':'right'}
    for lr in gprs:
        ik_file_data[lr_long[lr]] = gprs[lr].get_rel_transform(lr_long[lr], 'tool_tip')    

    
    yellowprint("Saving IK feedback data in file...")
    ik_file = osp.join(feedback_dir, ik_feedback_name)
    with open(ik_file,'w') as fh: cPickle.dump(ik_file_data, fh)
    yellowprint("Saved!")
コード例 #16
0
def record_demo (demo_name, use_voice):
    """
    Record a single demo.
    Returns True/False depending on whether demo needs to be saved.
    
    @demo_dir: directory where demo is recorded.
    @use_voice: bool on whether to use voice for demo or not.
    """
    global cmd_checker, topic_writer, demo_type_dir
    # Start here. Change to voice command.
    sleeper = rospy.Rate(30)
    demo_dir = osp.join(demo_type_dir, demo_name)

    started_video = {cam:False for cam in camera_types}

    print
    yellowprint("Starting bag file recording.")
    bag_file = osp.join(demo_dir,demo_names.bag_name)
    topic_writer.start_saving(bag_file)
    started_bag = True

    for cam in camera_types:
        yellowprint("Calling saveImagecamera%i service."%cam)
        save_image_services[cam](cam_save_requests[cam])
        started_video[cam] = True
    
    # Change to voice command
    time.sleep(1.2)
    subprocess.call("espeak -v en 'Recording.'", stdout=devnull, stderr=devnull, shell=True)
    time_start = time.time()
    if use_voice:
        while True:
            status = cmd_checker.get_latest_msg()
            if  status in ["cancel recording","finish recording","check demo"]:
                break
            sleeper.sleep()
    else:
        raw_input(colorize("Press any key when done.",'y',True))
    
    time_finish = time.time()
    
    for cam in started_video:
        if started_video[cam]:
            save_image_services[cam](cam_stop_request)
            yellowprint("Stopped video%i."%cam)
    if started_bag:
        topic_writer.stop_saving()
        yellowprint("Stopped bag.")
    
    
    if use_voice:
        while status == "check demo":
            subprocess.call("espeak -v en 'Visualizing.'", stdout=devnull, stderr=devnull, shell=True)
            view_hydra_demo_on_rviz(demo_type, demo_name, freq, speed, prompt=False, verbose=False)
            time.sleep(0.5)
            status = cmd_checker.get_latest_msg()
        greenprint("Time taken to record demo: %02f s"%(time_finish-time_start))
        return status == "finish recording"
    elif yes_or_no("Save demo?"):
        greenprint("Time taken to record demo: %02f s"%(time_finish-time_start))
        return True
    else:
        return False
コード例 #17
0
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        sleeper = rospy.Rate(30)
        
        avg_tfms = {}
        pot_avg = 0.0
        
        j = 0
        n_attempts_max = n_avg*2
        n_attempts_effective = 0;
        while j < n_attempts_max:
            j += 1
            tfms = {}
            # F**k the frames bullshit
            ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers, camera=1)
            hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
            pot_angle = gmt.get_pot_angle(self.lr)
            
            if not ar_tfm or (not hyd_tfm and self.hydras):
                if not ar_tfm:
                    yellowprint('Could not find required ar markers from '+str(self.ar_markers))
                else:
                    yellowprint('Could not find required hydra transforms from '+str(self.hydras))
                continue

            pot_avg += pot_angle
            tfms.update(ar_tfm)
            tfms.update(hyd_tfm)
            
            
            blueprint('\tGetting averaging transform: %d of %d ...'%(n_attempts_effective, n_avg-1))
            n_attempts_effective += 1

            # The angle relevant for each finger is only half the angle.                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
                
            if n_attempts_effective == n_avg:
                break
            
            sleeper.sleep()
            
        if n_attempts_effective < n_avg:
            return False;
            
        pot_avg /= n_avg
        
        greenprint("Average angle found: %f"%pot_avg)

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])
        
        ## only one marker on gripper now
        #if len(avg_tfms) == 1:
        #    yellowprint('Found %i marker only. Not enough to update.'%avg_tfms.keys()[0])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
        return True