def generate_target_course( x, y ): #tar manuelt inmatate coordinater och skapar ett polynom som blir referens! csp = cubic_spline_planner.Spline2D(x, y) s = np.arange(0, csp.s[-1], 0.1) d = np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, 0.1) #Skapa 0.1 tät vinkelrät linje s_len = s.size d_len = d.size LUTs = np.zeros((s_len, d_len)) LUTd = np.zeros((s_len, d_len)) LUTx = np.zeros((s_len, d_len)) LUTy = np.zeros((s_len, d_len)) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = csp.calc_position( i_s ) #i_s = incremental s ->här kan vi göra visualitionsconstraintsen rx.append(ix) #Ref x ry.append(iy) #Ref y ryaw.append(csp.calc_yaw(i_s)) #Ref yaw rk.append(csp.calc_curvature(i_s)) #Ref curvature LTs, LTd, LTx, LTy, refx, refy, refyaw = [], [], [], [], [], [], [] s_count, d_count = -1, -1 for i_ss in s: s_count = s_count + 1 LTs = i_ss refx, refy = csp.calc_position(i_ss) refyaw = csp.calc_yaw(i_ss) for i_dd in d: if i_dd == -MAX_ROAD_WIDTH: d_count = -1 d_count = d_count + 1 LTd = i_dd LTx = refx + i_dd * math.sin(refyaw) LTy = refy - i_dd * math.cos(refyaw) LUTs[s_count, d_count] = LTs LUTd[s_count, d_count] = LTd LUTx[s_count, d_count] = LTx LUTy[s_count, d_count] = LTy ''' print('Matrix Analysis ') print(len(LUTs)) print(len(LUTx)) print(LUTs) print(LUTd) print(LUTx) print(LUTy) ''' plt.plot(LUTx[:, :], LUTy[:, :]) plt.grid(True) plt.show() return rx, ry, ryaw, rk, csp, LUTs, LUTd, LUTx, LUTy
def generate_target_course(x, y): csp = cubic_spline_planner.Spline2D(x, y) s = np.arange(0, csp.s[-1], 0.1) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = csp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(csp.calc_yaw(i_s)) rk.append(csp.calc_curvature(i_s)) return rx, ry, ryaw, rk, csp
def generate_target_course(x, y): csp = cubic_spline_planner.Spline2D(x, y) s = np.arange(0, csp.s[-1], 0.1) d = np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, 0.1) #Creates a 0.1 perp. line s_len = s.size d_len = d.size LUTs = np.zeros((s_len, d_len)) LUTd = np.zeros((s_len, d_len)) LUTx = np.zeros((s_len, d_len)) LUTy = np.zeros((s_len, d_len)) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = csp.calc_position( i_s) #i_s = incremental s ->Visual constraints rx.append(ix) #Ref x ry.append(iy) #Ref y ryaw.append(csp.calc_yaw(i_s)) #Ref yaw rk.append(csp.calc_curvature(i_s)) #Ref curvature LTs, LTd, LTx, LTy, refx, refy, refyaw = [], [], [], [], [], [], [] s_count, d_count = -1, -1 for i_ss in s: s_count = s_count + 1 LTs = i_ss refx, refy = csp.calc_position(i_ss) refyaw = csp.calc_yaw(i_ss) for i_dd in d: if i_dd == -MAX_ROAD_WIDTH: d_count = -1 d_count = d_count + 1 LTd = i_dd LTx = refx + i_dd * math.sin(refyaw) LTy = refy - i_dd * math.cos(refyaw) LUTs[s_count, d_count] = LTs LUTd[s_count, d_count] = LTd LUTx[s_count, d_count] = LTx LUTy[s_count, d_count] = LTy print('Matrix Analysis ') print(len(LUTs)) print(len(LUTx)) print(LUTs) print(LUTd) print(LUTx) print(LUTy) plt.plot(LUTx[:, :], LUTy[:, :]) #plt.plot(LUTx[:,-1], LUTy[:,-1]) plt.grid(True) plt.show() return rx, ry, ryaw, rk, csp, LUTs, LUTd, LUTx, LUTy
def generate_coarse_course(x, y): csp = cubic_spline_planner.Spline2D(x, y) s = np.arange(0, csp.s[-1], 1) # 2m取一个间隔点,利用dp进行基于reference line的轨迹优化, 并保证一定的计算速度 rx, ry, ryaw, rk = [], [], [], [] for i_s in s: # 利用离散点x、y生成一条关于路程s的曲线? 然后离散成x、y、yaw、curvature ix, iy = csp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(csp.calc_yaw(i_s)) rk.append(csp.calc_curvature(i_s)) return rx, ry, ryaw, rk, csp
def generate_target_course(x, y): ''' 根据way points利用分段三次样条多项式生成平滑参考轨迹 :param x: :param y: :return: 参考信息[x,y,yaw,c,csp] csp:样条曲线类,内包含求位置、曲率、航向角的函数 ''' csp = cubic_spline_planner.Spline2D(x, y) s = np.arange(0, csp.s[-1], 0.1) rx, ry, ryaw, rk = [], [], [], [] for i_s in s: ix, iy = csp.calc_position(i_s) rx.append(ix) ry.append(iy) ryaw.append(csp.calc_yaw(i_s)) rk.append(csp.calc_curvature(i_s)) return rx, ry, ryaw, rk, csp
def __init__(self): print("Initializaing Robot") # Position Variables # Will replace with actual values self.x = 150 self.y = 0 self.yaw = 180 # Driving Variables self.target_power = 50 self.target_velocity = 60 self.v = 0 self.max_turning_angle = 70 # Going straight, >0=CCW, <0=CW self.min_turn_radius = 0.25 # [m] self.wheel_base = 20.32 # Navigation Variables self.state = HOME self.circle_completion_state = False self.current_circle = 0 self.spline = spline.Spline2D([0, 1], [0, 1]) #Condition Variables self.object_count = 0 self.beam_state = True self.end_time = False self.off_track = False self.start_time = time.time() #UDP Server #Sets the io scheme to be based on board pin numbers #Use io.BCM for GPIO based classifications io.setmode(io.BOARD) # Motor initialization---------------------------------------------------------- self.PWM_MAX = 100 io.setwarnings(False) self.leftMotor_DIR_pin = 15 io.setup(self.leftMotor_DIR_pin, io.OUT) self.rightMotor_DIR_pin = 16 io.setup(self.rightMotor_DIR_pin, io.OUT) io.output(self.leftMotor_DIR_pin, False) io.output(self.rightMotor_DIR_pin, False) self.leftMotor_PWM_pin = 11 self.rightMotor_PWM_pin = 12 io.setup(self.leftMotor_PWM_pin, io.OUT) io.setup(self.rightMotor_PWM_pin, io.OUT) # MAX Frequency 20 Hz self.leftMotorPWM = io.PWM(self.leftMotor_PWM_pin, 1000) self.rightMotorPWM = io.PWM(self.rightMotor_PWM_pin, 1000) self.leftMotorPWM.start(0) self.leftMotorPWM.ChangeDutyCycle(0) self.rightMotorPWM.start(0) self.rightMotorPWM.ChangeDutyCycle(0) self.leftMotorPower = 0 self.rightMotorPower = 0 # Beam Break Setup---------------------------------------------------------------- io.setmode(io.BOARD) io.setup(40, io.IN)
def generate_path(self): x = self.x y = self.y c = self.current_circle #print("Current Circle: "+ str(c)) state = self.state #If returned from circle, move to turnaround state if (state == PATHING_HOME): print("Arrived home, turning arround") self.state = TURNAROUND self.object_count = 0 return elif (state == TURNAROUND): print( "Turned around, currently at home preparing to path back out") self.x = 150 self.y = 0 self.yaw = 180 self.state = HOME return #Else state is either HOME or PATHING_CIRCLE else: #If at home, path back into circle if (state == HOME): if (self.end_time == True): endgame() return if (c >= 5): c = 4 print("Pathing out of home to Circle: " + str(self.current_circle)) self.state = PATHING_CIRCLE cx = [ x, circleRadii[c + 1], circleXY[c], 0, -circleXY[c], -circleRadii[c], -circleXY[c], 0, circleXY[c] ] cy = [ y, 0, circleXY[c], circleRadii[c], circleXY[c], 0, -circleXY[c], -circleRadii[c], -circleXY[c] ] #If pathing circle, either return home or continue to next circle depending on the check states elif (state == PATHING_CIRCLE): #If bay area is full or time is out if (self.object_count >= 3 or self.end_time == True): print("Bay area full or time ran out, returning home") self.state = PATHING_HOME if (c >= 5): c = 4 cx = [x, circleRadii[c + 1], 150] cy = [y, 0, 0] self.current_circle += 1 #Continue to next circle else: target_circle = c + 1 if (target_circle <= 4 and not self.circle_completion_state): print("Pathing from Circle: " + str(self.current_circle) + " around Circle: " + str(target_circle)) cx = [ x, circleRadii[c], circleXY[c], 0, -circleXY[c], -circleRadii[c], -circleXY[c], 0, circleXY[c] ] cy = [ y, 0, circleXY[c], circleRadii[c], circleXY[c], 0, -circleXY[c], -circleRadii[c], -circleXY[c] ] self.current_circle += 1 else: if (not self.circle_completion_state): print( "All circles pathed, now looping around Circle: 0" ) self.circle_completion_state = True self.current_circle = 0 cx = [ x, circleRadii[0], circleXY[0], 0, -circleXY[0], -circleRadii[0], -circleXY[0], 0, circleXY[0] ] cy = [ y, 0, circleXY[0], circleRadii[0], circleXY[0], 0, -circleXY[0], -circleRadii[0], -circleXY[0] ] self.spline = spline.Spline2D(cx, cy)