def Plan_LocalPath(self, PrevRange): ''' Plans the local course over the next waypoint Contains a straight path and the required turn Outputs a list of SWP-s ''' self.Current_SWP = self.SegmentCoords self.NextSWP_No = 0 self.NextSWP_validity = 0 gamma = 0 i = 0 while gamma <= 0 or gamma >= math.pi: i = i + 1 '''Turning waypoint''' n = self.LastWP + 1 + i wpnum = len(self.Waypoints.get_WayPoints()[0]) print(wpnum, n) try: if n + 1 >= wpnum: Nm = self.Waypoints.get_SingleWayPoint(n - i) Nt = self.Waypoints.get_SingleWayPoint(n) Np = self.retpos else: Nm = self.Waypoints.get_SingleWayPoint(n - i) Nt = self.Waypoints.get_SingleWayPoint(n) Np = self.Waypoints.get_SingleWayPoint(n + 1) except IndexError: return (-1) gamma = FL.CosLaw(Nm, Nt, Np) self.Path = OL.O_LocalPath(gamma, self.Sigma_max, self.Kappa_max) definition = 20 self.Path.FitPath(definition / 4) self.Path.PositionPoly(Nm, Nt, Np) '''fitline''' Range = self.Path.get_Range() self.Straight = OL.O_StraightPath(Nt, Nm, Range, PrevRange) self.Straight.FitLine(0.07) return i
def get_Thera_r(self): ''' Calculates the required heading ''' Theta_r = FL.CosLaw(self.Pos.Extend_Zero(), self.Pos, self.NextSWP) ''' The CosLaw function returns a positive angle If the ship must turn left, the X coordinate of the Next SWP < X coordinate of Ship In that case, Theta_r is in the negative angle-space, therefore must be negated ''' if self.NextSWP.get_Pos_X() < self.Pos.get_Pos_X(): Theta_r *= -1 return Theta_r
def Plan_LocalPath(self, PrevRange): ''' Plans the local course over the next waypoint Contains a straight path and the required turn Outputs a list of SWP-s ''' self.Current_SWP = self.SegmentCoords; self.NextSWP_No = 0; self.NextSWP_validity = 0; gamma = 0; i = 0; while gamma <= 0 or gamma >= math.pi: i = i + 1; '''Turning waypoint''' n = self.LastWP + 1 + i; wpnum = len(self.Waypoints.get_WayPoints()[0]) print (wpnum,n) try: if n+1 >= wpnum: Nm = self.Waypoints.get_SingleWayPoint(n - i); Nt = self.Waypoints.get_SingleWayPoint(n); Np = self.retpos; elif n-i < 0: Nm = self.Pos Nt = self.Waypoints.get_SingleWayPoint(n); Np = self.Waypoints.get_SingleWayPoint(n + 1); else: Nm = self.Waypoints.get_SingleWayPoint(n - i); Nt = self.Waypoints.get_SingleWayPoint(n); Np = self.Waypoints.get_SingleWayPoint(n + 1); except IndexError: #print 'szopo' return(-1); gamma = FL.CosLaw(Nm, Nt, Np); self.Path = OL.O_LocalPath(gamma, self.Sigma_max, self.Kappa_max); definition = 30; self.Path.FitPath(definition/8); self.Path.PositionPoly(Nm, Nt, Np); '''fitline''' Range = self.Path.get_Range(); self.Straight = OL.O_StraightPath(Nt, Nm, Range, PrevRange); self.Straight.FitLine(definition); return i;