===== T2b-dtspn - Data collection path planning with curvature-constrained trajectory - Dubins TSPN ======

The main task is to implement two approaches solving the curvature-constrained Traveling Salesman Problem with neighborhoods.

|**Deadline**        |  November 30th, 2019, 23:59 PST |
|**Points**          |                               5 |
|**Label in BRUTE**  |                       T2b-dtspn |
|**Files to submit** |  archive with ''DTSPNSolverDecoupled.py'' and ''DTSPNSolverNoonBean.py'' |
|**Resources**  |     {{ :courses:b4m36uir:hw:uir-t2b-dtspn.zip | T2b-dtspn resource pack (updated 22.11.19) }} |

\\

==== Decoupled Approach ====

In the ''DTSPNSolverDecoupled.plan_tour'' function implement the decoupled solution for the Dubins TSP with Neighborhoods (DTSPN) with disk-shaped regions.

The decoupled approach comprises the following basic steps
    - Estimate sequence of visits by Euclidean TSP connecting centers of the regions.
    - For each region, sample boundary points and heading angles.
    - Find the shortest feasible tour comprising Dubins maneuvers connecting the regions, where the sequence of visits is estimated from the ETSP.

Notice, the number of heading and position samples is not defined. Your task is also to try various values and submit a code with a reasonable number of samples.

The ''DTSPNSolverDecoupled.plan_tour'' function has the following prescription
<code python>

class DTSPNSolverDecoupled(DTSPNSolver.DTSPNSolver):

    ...

    def plan_tour(self, goals, sensing_radius, turning_radius):
    """
    Compute a DTSPN tour using the decoupled approach.  
 
    Parameters
    ----------
    goals: list (float, float)
        list of the TSP goal coordinates  
    sensing_radius: float
        neighborhood of TSP goals  
    turning_radius: float
        turning radius for the Dubins vehicle model  
        
    Returns
    -------
    list (float,float,float)
        tour as a list of robot configurations (x,y,phi), each corresponding to a TSP goal
    """

    n = len(goals)
    self.distances = np.zeros((n,n))	
    self.paths = {}
    '''
    TODO - homework
        - Compute the distance between the individual goals  
    '''
    print ('TODO distances')
    self.distances = np.ones(self.distances.shape)


    sequence = self.compute_TSP_sequence()
        
    '''
    TODO - homework
        - Sample the configurations ih the goal areas
        - Find the shortest tour
    '''
    print ('TODO sampling')
    selected_configurations = []
    for a in range(n):
        selected_configurations.append ( ( goals[sequence[a]][0], goals[sequence[a]][1], math.pi ) )
    return selected_configurations

</code>

\\


==== Noon-Bean Transform Approach ====

In the ''DTSPNSolverNoonBean.plan_tour'' function implement the Noon-Bean trasnform solution for the Dubins TSP with Neighborhoods (DTSPN) with disk-shaped regions.

The Noon-Bean approach comprises the following basic steps
    - For each region, sample boundary points and heading angles.
    - Construct a distance  matrix using the Noon-Bean transform ({{courses:b4m36uir:lectures:b4m36uir-lec05-slides.pdf| Lecture 5. Multi-goal (data collection) planning}}) where the individual regions correspond to the NoonBean's Generalized TSP sets.
    - Find the shortest feasible tour created from Dubins maneuvers by solving the Asymmetric TSP problem characterized by the distance matrix.

Notice, the number of heading and position samples is not defined. Your task is also to try various values and submit a code with a reasonable number of samples.

The ''DTSPNSolverNoonBean.plan_tour'' function has the following prescription
<code python>

class DTSPNSolverNoonBean(DTSPNSolver.DTSPNSolver):

    ...

    def plan_tour(self, goals, sensing_radius, turning_radius):
        """
        Compute a DTSPN tour using the NoonBean approach.  
 
        Parameters
        ----------
        goals: list (float, float)
            list of the TSP goal coordinates  
        sensing_radius: float
            neighborhood of TSP goals  
        turning_radius: float
            turning radius for the Dubins vehicle model  
 
        Returns
        -------
        list (float,float,float)
            tour as a list of robot configurations (x,y,phi), each corresponding to a TSP goal
        """

        n = len(goals)
        self.distances = np.zeros((n,n))	
        
        '''
        TODO - Noon-Bean approach
        '''

        print ('TODO - Noon-Bean')

        configurations = []
        for goal in goals:
            configurations.append((goal[0],goal[1],math.pi))
            
        return configurations
</code>

\\
  
==== Appendix ====
\\
  
=== Installation of the Prepared Dependencies ===

First, download prepared codes and configuration files. 
Then, download and compile the LKH solver (implementation of the Lin–Kernighan heuristic algorithm) and the GDIP Dubins library as follows

<code bash>
cd lkh
./install.sh
cd -

cd gdip
./install.sh
cd -
</code>

\\

=== Dubins maneuver ===
[[https://en.wikipedia.org/wiki/Dubins_path]]

\\

=== Generalized Dubins Interval Problem ===
[[https://github.com/comrob/gdip]]