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;
