===== 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 27, 2022, 23:59 PST   |
|**Points**          |                               7 |
|**Label in BRUTE**  |                       t2b-dtspn |
|**Files to submit** |  archive with ''DTSPNSolver.py'' |
|**Resources**  |  {{ :courses:uir:hw:b4m36uir_22_t2_resource_pack.zip |B4M36UIR_t2_resource_pack  }} |
|               |  {{ :courses:uir:hw:lkh-2.0.9.tgz |LKH updated for gcc 11 [credit for fix: Petr Brož] }} |

\\

==== Necessary information in Lectures ====

{{courses:uir:lectures:b4m36uir-lec05-slides.pdf| 5. Multi-goal Path planning}}

{{courses:uir:lectures:b4m36uir-lec06-slides.pdf| 6. Data Collection Planning}}

{{courses:uir:lectures:b4m36uir-lec07-slides.pdf| 7. Curvature-constrained Data collection Planning}}

==== Decoupled Approach ====

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

{{:courses:uir:hw:screenshot_from_2020-11-16_14-04-03.png?500|}}
{{:courses:uir:hw:screenshot_from_2020-11-16_14-07-57.png?200|}}

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 ''plan_tour_decoupled'' function has the following prescription:
<code python>
def plan_tour_decoupled(goals, sensing_radius, turning_radius):
    """
    Compute a DTSPN tour using the decoupled approach.  

    Parameters
    ----------
    goals: list Vector3
        list of the TSP goal coordinates (x, y, 0)
    sensing_radius: float
        neighborhood of TSP goals  
    turning_radius: float
        turning radius for the Dubins vehicle model  

    Returns
    -------
    Path, path_length (float)
        tour as a list of robot configurations in SE3 densely sampled
    """

</code>


\\


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

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

{{:courses:uir:hw:screenshot_from_2020-11-16_14-10-55.png?500|}}

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 (from lectures) 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 ''plan_tour_noon_bean'' function has the following prescription

<code python>
def plan_tour_noon_bean(goals, sensing_radius, turning_radius):
    """
    Compute a DTSPN tour using the NoonBean approach.  

    Parameters
    ----------
    goals: list Vector3
        list of the TSP goal coordinates (x, y, 0)
    sensing_radius: float
        neighborhood of TSP goals  
    turning_radius: float
        turning radius for the Dubins vehicle model  

    Returns
    -------
    Path, path_length (float)
        tour as a list of robot configurations in SE3 densely sampled
    """

</code>

<note important>The LKH solver seems to be very sensitive to the choice of bigM constant for Noon-Bean transformation. It influences the computational time significantly. The lower the bigM constant is, the faster it computes; but if it is too small, it can violate the original assumption to visit each set only once. The reference sets bigM to be the length of the longest Dubins path and use it also for representing infinity. The distance matrix is normalized before solving by LKH, and thus it cannot contains infinity values. (Alternatively, you can use a slightly smaller number of samples.)</note>

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

1) Download prepared codes and configuration files. 

2) Install 'dubins' package to python3.

<code bash>
pip3 install dubins # or our favorite way to install the package
</code>

3) Compile the LKH solver (implementation of the Lin–Kernighan heuristic algorithm) and the GDIP Dubins library as follows

<code bash>
./install_lkh.sh
</code>

Since both the dubins package and the LKH wrapper used the second part of the tasj use SE(2) coordinates represented as tuples, ''pose_to_se2'' are ''se2_to_pose'' functions are provided for you convenience:
<code python>
def pose_to_se2(pose):
    return pose.position.x,pose.position.y,pose.orientation.to_Euler()[0]

def se2_to_pose(se2):
    pose = Pose()
    pose.position.x = se2[0]
    pose.position.y = se2[1]
    pose.orientation.from_Euler(se2[2],0,0) 
    return pose
</code>


\\

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