package updates

README.md

@@ -4,11 +4,14 @@
 [![PyPi license](https://img.shields.io/pypi/l/ansicolortags.svg)](https://pypi.org/project/cc-pathplanner/0.1.0/)
 
 
-This repository contains a program which generates a guidance trajectory for complete 2D coverage. It can be used for operations where complete coverage of an Area of Interest (AoI) is required, for land or marine applications.
+## Getting started
+This repository contains a program which generates a guidance trajectory for complete 2D coverage. It can be used for operations where complete coverage of an Area of Interest (AoI) is required for various applications.
+The package can be installed from PyPi by running 'pip install covplan'
+
 
 ## How to use the program
-To use the program, run `coverage_path_planner.cpp(input_file, params)`. It returns a list of coordinates that compose a path for complete coverage.
-In the input file, describe the boundaries of the AoI using its lat-lon coordinates like this:  
+To use the program, run `coverage_path_planner.covplan(input_file, params)`. It returns a list of coordinates that compose a path for complete coverage.
+In the input file, describe the boundaries of the AoI using its lat-lon coordinates in the following format:  
 ```
   lat1  lon1
   lat2  lon2
@@ -21,7 +24,7 @@ Ensure that the AoI is a closed polygon, by keeping the first coordinate the sam
 
 ## Example usage
 ```python
-from cc_pathplanner import coverage_path_planner
+from covplan import coverage_path_planner
 
 def main():
 	n_clusters=4	#number of sections
@@ -31,10 +34,11 @@ def main():
 	driving_angle=90	#angle wrt X-axis in degrees
 	no_hd=0	#number of margins around boundary (each with distance=0.5*width) if needed, otherwise 0
 
-	op=coverage_path_planner.cpp(input_file,num_hd=no_hd,width=width,theta=driving_angle,num_clusters=n_clusters,radius=r,visualize=False) # returns list of waypoint coordinates composing trajectory
+	op=coverage_path_planner.covplan(input_file,num_hd=no_hd,width=width,theta=driving_angle,num_clusters=n_clusters,radius=r,visualize=False) # returns list of waypoint coordinates composing full trajectory for coverage
 	print('The trajectory for full coverage consists of the following waypoints:',op)
-	min=coverage_path_planner.find_min(input_file)  # runs optimizer and returns angle corresponding to minimum path length
-	print('Angle for trajectory with minimum length:', min)
+
+	min=coverage_path_planner.find_min(input_file,width=width,num_hd=no_hd,num_clusters=n_clusters,radius=r,verbose=True)  # runs optimizer and returns angle corresponding to minimum path length
+	# print('Angle for trajectory with minimum length:', min)
 
 if __name__ == '__main__':
 	main()

src/cc_pathplanner/angles.py → build/lib/cc_pathplanner/angles.py

@@ -1,7 +1,6 @@
 import numpy as np
 from scipy.spatial.transform import Rotation as Rot
 
-
 def rot_mat_2d(angle):
     """
     Create 2D rotation matrix from an angle

src/cc_pathplanner/field.py → build/lib/cc_pathplanner/field.py

@@ -9,7 +9,7 @@
 from cc_pathplanner.dubins_path_planner import plan_dubins_path
 from cc_pathplanner.lines import computeAngle, pointOnLine, intersectLines, intersectPointOnLine
 from cc_pathplanner.lines import getPoly, numPoly
-# from convert_coordinates import LLtoUTM, UTMtoLL
+# from LLcoordinates import LLtoUTM, UTMtoLL
 import folium
 import utm
 

build/lib/covplan/angles.py

@@ -0,0 +1,70 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as Rot
+
+def rot_mat_2d(angle):
+    """
+    Create 2D rotation matrix from an angle
+    Parameters
+    ----------
+    angle :
+    Returns
+    -------
+    A 2D rotation matrix
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+    """
+    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
+
+
+def angle_mod(x, zero_2_2pi=False, degree=False):
+    """
+    Angle modulo operation
+    Default angle modulo range is [-pi, pi)
+    Parameters
+    ----------
+    x : float or array_like
+        A angle or an array of angles. This array is flattened for
+        the calculation. When an angle is provided, a float angle is returned.
+    zero_2_2pi : bool, optional
+        Change angle modulo range to [0, 2pi)
+        Default is False.
+    degree : bool, optional
+        If True, then the given angles are assumed to be in degrees.
+        Default is False.
+    Returns
+    -------
+    ret : float or ndarray
+        an angle or an array of modulated angle.
+    Examples
+    --------
+    >>> angle_mod(-4.0)
+    2.28318531
+    >>> angle_mod([-4.0])
+    np.array(2.28318531)
+    >>> angle_mod([-150.0, 190.0, 350], degree=True)
+    array([-150., -170.,  -10.])
+    >>> angle_mod(-60.0, zero_2_2pi=True, degree=True)
+    array([300.])
+    """
+    if isinstance(x, float):
+        is_float = True
+    else:
+        is_float = False
+
+    x = np.asarray(x).flatten()
+    if degree:
+        x = np.deg2rad(x)
+
+    if zero_2_2pi:
+        mod_angle = x % (2 * np.pi)
+    else:
+        mod_angle = (x + np.pi) % (2 * np.pi) - np.pi
+
+    if degree:
+        mod_angle = np.rad2deg(mod_angle)
+
+    if is_float:
+        return mod_angle.item()
+    else:
+        return mod_angle

build/lib/covplan/convert_coordinates.py

@@ -0,0 +1,212 @@
+# from http://www.geo.uib.no/polarhovercraft/uploads/Main/LatLongUTMconversion.py
+# Lat Long - UTM, UTM - Lat Long conversions
+
+from math import pi, sin, cos, tan, sqrt
+
+#LatLong- UTM conversion..h
+#definitions for lat/long to UTM and UTM to lat/lng conversions
+#include <string.h>
+
+_deg2rad = pi / 180.0
+_rad2deg = 180.0 / pi
+
+_EquatorialRadius = 2
+_eccentricitySquared = 3
+
+_ellipsoid = [
+#  id, Ellipsoid name, Equatorial Radius, square of eccentricity	
+# first once is a placeholder only, To allow array indices to match id numbers
+	[ -1, "Placeholder", 0, 0],
+	[ 1, "Airy", 6377563, 0.00667054],
+	[ 2, "Australian National", 6378160, 0.006694542],
+	[ 3, "Bessel 1841", 6377397, 0.006674372],
+	[ 4, "Bessel 1841 (Nambia] ", 6377484, 0.006674372],
+	[ 5, "Clarke 1866", 6378206, 0.006768658],
+	[ 6, "Clarke 1880", 6378249, 0.006803511],
+	[ 7, "Everest", 6377276, 0.006637847],
+	[ 8, "Fischer 1960 (Mercury] ", 6378166, 0.006693422],
+	[ 9, "Fischer 1968", 6378150, 0.006693422],
+	[ 10, "GRS 1967", 6378160, 0.006694605],
+	[ 11, "GRS 1980", 6378137, 0.00669438],
+	[ 12, "Helmert 1906", 6378200, 0.006693422],
+	[ 13, "Hough", 6378270, 0.00672267],
+	[ 14, "International", 6378388, 0.00672267],
+	[ 15, "Krassovsky", 6378245, 0.006693422],
+	[ 16, "Modified Airy", 6377340, 0.00667054],
+	[ 17, "Modified Everest", 6377304, 0.006637847],
+	[ 18, "Modified Fischer 1960", 6378155, 0.006693422],
+	[ 19, "South American 1969", 6378160, 0.006694542],
+	[ 20, "WGS 60", 6378165, 0.006693422],
+	[ 21, "WGS 66", 6378145, 0.006694542],
+	[ 22, "WGS-72", 6378135, 0.006694318],
+	[ 23, "WGS-84", 6378137, 0.00669438]
+]
+
+#Reference ellipsoids derived from Peter H. Dana's website- 
+#http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
+#Department of Geography, University of Texas at Austin
+#Internet: [email protected]
+#3/22/95
+
+#Source
+#Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
+#1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
+
+#def LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long, 
+#			 double &UTMNorthing, double &UTMEasting, char* UTMZone)
+
+def LLtoUTM(ReferenceEllipsoid, Lat, Long):
+#converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
+#East Longitudes are positive, West longitudes are negative. 
+#North latitudes are positive, South latitudes are negative
+#Lat and Long are in decimal degrees
+#Written by Chuck Gantz- [email protected]
+
+    a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
+    eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
+    k0 = 0.9996
+
+#Make sure the longitude is between -180.00 .. 179.9
+    LongTemp = (Long+180)-int((Long+180)/360)*360-180 # -180.00 .. 179.9
+
+    LatRad = Lat*_deg2rad
+    LongRad = LongTemp*_deg2rad
+
+    ZoneNumber = int((LongTemp + 180)/6) + 1
+
+    if Lat >= 56.0 and Lat < 64.0 and LongTemp >= 3.0 and LongTemp < 12.0:
+        ZoneNumber = 32
+
+    # Special zones for Svalbard
+    if Lat >= 72.0 and Lat < 84.0:
+        if  LongTemp >= 0.0  and LongTemp <  9.0:ZoneNumber = 31
+        elif LongTemp >= 9.0  and LongTemp < 21.0: ZoneNumber = 33
+        elif LongTemp >= 21.0 and LongTemp < 33.0: ZoneNumber = 35
+        elif LongTemp >= 33.0 and LongTemp < 42.0: ZoneNumber = 37
+
+    LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 #+3 puts origin in middle of zone
+    LongOriginRad = LongOrigin * _deg2rad
+
+    #compute the UTM Zone from the latitude and longitude
+    UTMZone = "%d%c" % (ZoneNumber, _UTMLetterDesignator(Lat))
+
+    eccPrimeSquared = (eccSquared)/(1-eccSquared)
+    N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad))
+    T = tan(LatRad)*tan(LatRad)
+    C = eccPrimeSquared*cos(LatRad)*cos(LatRad)
+    A = cos(LatRad)*(LongRad-LongOriginRad)
+
+    M = a*((1
+            - eccSquared/4
+            - 3*eccSquared*eccSquared/64
+            - 5*eccSquared*eccSquared*eccSquared/256)*LatRad 
+           - (3*eccSquared/8
+              + 3*eccSquared*eccSquared/32
+              + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
+           + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
+           - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad))
+
+    UTMEasting = (k0*N*(A+(1-T+C)*A*A*A/6
+                        + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+                  + 500000.0)
+
+    UTMNorthing = (k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+                                        + (61
+                                           -58*T
+                                           +T*T
+                                           +600*C
+                                           -330*eccPrimeSquared)*A*A*A*A*A*A/720)))
+
+    if Lat < 0:
+        UTMNorthing = UTMNorthing + 10000000.0; #10000000 meter offset for southern hemisphere
+    return (UTMZone, UTMEasting, UTMNorthing)
+
+
+def _UTMLetterDesignator(Lat):
+#This routine determines the correct UTM letter designator for the given latitude
+#returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+#Written by Chuck Gantz- [email protected]
+
+    if 84 >= Lat >= 72: return 'X'
+    elif 72 > Lat >= 64: return 'W'
+    elif 64 > Lat >= 56: return 'V'
+    elif 56 > Lat >= 48: return 'U'
+    elif 48 > Lat >= 40: return 'T'
+    elif 40 > Lat >= 32: return 'S'
+    elif 32 > Lat >= 24: return 'R'
+    elif 24 > Lat >= 16: return 'Q'
+    elif 16 > Lat >= 8: return 'P'
+    elif  8 > Lat >= 0: return 'N'
+    elif  0 > Lat >= -8: return 'M'
+    elif -8> Lat >= -16: return 'L'
+    elif -16 > Lat >= -24: return 'K'
+    elif -24 > Lat >= -32: return 'J'
+    elif -32 > Lat >= -40: return 'H'
+    elif -40 > Lat >= -48: return 'G'
+    elif -48 > Lat >= -56: return 'F'
+    elif -56 > Lat >= -64: return 'E'
+    elif -64 > Lat >= -72: return 'D'
+    elif -72 > Lat >= -80: return 'C'
+    else: return 'Z'	# if the Latitude is outside the UTM limits
+
+#void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
+#			  double& Lat,  double& Long )
+
+def UTMtoLL(ReferenceEllipsoid, northing, easting, zone):
+
+#converts UTM coords to lat/long.  Equations from USGS Bulletin 1532 
+#East Longitudes are positive, West longitudes are negative. 
+#North latitudes are positive, South latitudes are negative
+#Lat and Long are in decimal degrees. 
+#Written by Chuck Gantz- [email protected]
+#Converted to Python by Russ Nelson <[email protected]>
+
+    k0 = 0.9996
+    a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
+    eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
+    e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared))
+    #NorthernHemisphere; //1 for northern hemispher, 0 for southern
+
+    x = easting - 500000.0 #remove 500,000 meter offset for longitude
+    y = northing
+
+    ZoneLetter = zone[-1]
+    ZoneNumber = int(zone[:-1])
+    if ZoneLetter >= 'N':
+        NorthernHemisphere = 1  # point is in northern hemisphere
+    else:
+        NorthernHemisphere = 0  # point is in southern hemisphere
+        y -= 10000000.0         # remove 10,000,000 meter offset used for southern hemisphere
+
+    LongOrigin = (ZoneNumber - 1)*6 - 180 + 3  # +3 puts origin in middle of zone
+
+    eccPrimeSquared = (eccSquared)/(1-eccSquared)
+
+    M = y / k0
+    mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256))
+
+    phi1Rad = (mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 
+               + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
+               +(151*e1*e1*e1/96)*sin(6*mu))
+    phi1 = phi1Rad*_rad2deg;
+
+    N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad))
+    T1 = tan(phi1Rad)*tan(phi1Rad)
+    C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad)
+    R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5)
+    D = x/(N1*k0)
+
+    Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
+                                          +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720)
+    Lat = Lat * _rad2deg
+
+    Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
+            *D*D*D*D*D/120)/cos(phi1Rad)
+    Long = LongOrigin + Long * _rad2deg
+    return (Lat, Long)
+
+if __name__ == '__main__':
+    (z, e, n) = LLtoUTM(23, 45.00, -75.00)
+    print (z, e, n)
+    (lat, lon) = UTMtoLL(23, n, e, z)
+    print (lat, lon)

build/lib/covplan/coverage_path_planner.py

@@ -0,0 +1,40 @@
+from covplan.field import Field
+from scipy import optimize
+
+def covplan(input_file,width,num_hd=0,theta=0,num_clusters=3,radius=2,visualize=True):
+
+	f = Field(input_file, width, num_hd, theta)
+	f.headlandGen()
+	f.trackGen()
+	f.cluster(num_clusters)
+	f.tsp_opt()
+	f.trajGen(radius)
+	print('Total distance = {}; Track length = {}'.format(f.track_len+f.turn_dist, f.track_len))
+
+	if visualize==True:
+		f.showField()
+
+	return f.latlon
+
+def find_min(input_file,width=10,num_hd=0,num_clusters=4,radius=2,verbose=False):
+
+    def get_dist(angle):
+        f = Field(input_file, width, num_hd, angle)
+        # f = Field('mfield.txt', 10, 0, 90)
+        f.headlandGen()
+        f.trackGen()
+        f.cluster(num_clusters)
+        f.tsp_opt()
+        f.trajGen(radius)    
+        return f.turn_dist+f.track_len
+
+
+    min=optimize.dual_annealing(get_dist,maxiter=100,args=(), bounds=[(0,90)])
+
+    if verbose:
+        print('The minimum path length is: ',min.fun)
+        print('The angle for the path with minimum length is: ', min.x)
+
+    return min.x
+
+