コード例 #1
0
def c_v(car_t, pt_t1, pt_o):
    """ 计算当前车辆与目标点之间Cv权重(角度变化导致的权重变化)
        如果car_t是新track,由于位置检测的漂移,方向飘忽不定,暂时不使用curr_v,
        由curr_d代替。
    """
    if False: #not car_t.is_new:
        v_t = car_t.curr_v
        v_t1 = (car_t.curr_xy.vec() - pt_t1.vec())/(car_t.step*car_t.interval)
        dot = np.dot(v_t, v_t1)
        t_modulus = modulus(v_t)
        t1_modulus = modulus(v_t1)
        cos_angle = dot/(t_modulus*t1_modulus) # cosinus of angle between x and y
        return 0.5 + 0.5*cos_angle
    else:
        D = 360 - car_t.curr_d
        Dt1 = 360 - pt_o
        #print D, np.cos(np.radians(D)), np.sin(np.radians(D))
        v_t = unit_vector(Point(np.cos(np.radians(D)), np.sin(np.radians(D))).vec())
        v_t1 = None
        if car_t.curr_xy.dist(pt_t1) < 20:
            v_t1 = unit_vector(Point(np.cos(np.radians(Dt1)), np.sin(np.radians(Dt1))).vec())
        else:        
        # TODO: 暂时修正为,单纯依靠检测方向
            v_t1 = unit_vector(car_t.curr_xy.vec() - pt_t1.vec())
        cos_angle = np.dot(v_t, v_t1) # 如果前后点不动会导致nan值
        if np.isnan(cos_angle):
            cos_angle = 1
        # print cos_angle, v_t, v_t1
        return 0.5 + 0.5*abs(cos_angle) #不管正负方向一致就行,初始的时候是这样的
コード例 #2
0
def save_v(car_list, fname):
    with file(fname, 'w') as f:
        f.writelines(['x,y,v\n'])
        for car in car_list:
            if 0 != len(car.hist_xy):
                f.writelines(['%s,'%car.curr_xy.X, '%s,'%car.curr_xy.Y, \
                            '%s\n'%(modulus(car.curr_xy.vec() - \
                            car.hist_xy[-1].vec())/(car.interval*car.step))])
コード例 #3
0
def save_v(car_list, fname):
    with file(fname, 'w') as f:
        f.writelines(['x,y,v\n'])
        for car in car_list:
            if 0 != len(car.hist_xy):
                # 计算速度,输出单位km/h
                # v = dist / time_interval
                v = 3.6*modulus(car.curr_xy.vec()-car.hist_xy[-1].vec())*_resolution_/(car.interval*car.step)
                f.writelines(['%s,'%car.curr_xy.X, '%s,'%car.curr_xy.Y, '%s\n'%v])
コード例 #4
0
def c_p(car_t, pt_t1):
    """ 计算当前车辆预期结果与目标点之间Cp权重(距离变化导致的权重变化)
          如果car_t是新track,需要改写
    """
    #x_t_k = car_t.curr_xy.vec()
    #v_t_k = car_t.curr_v
    #x_t_1 = pt_t1.vec()
    #k+1 = car_t.intervel
    if False: #not car_t.is_new:
        r_hat = modulus(car_t.curr_xy.vec() + car_t.interval * car_t.curr_v - pt_t1.vec())
        return 1 - r_hat / search_r
    else:
        return 1 - car_t.curr_xy.dist(pt_t1)/(search_r) #/car_t.interval