# `mrim` Project Planning
###### tags: `MRS 2022`

### Resources
* [Presentation](https://docs.google.com/presentation/d/14k9xkOqDO7CFfWA4GhnCWVrdFvwzGHGyKNompSISjF8/edit#slide=id.p)
* [Common Repository](https://github.com/Peter010103/MRS-2022)
* [Minimum snap trajectory implementation](https://github.com/The-SS/quadrotor_trajectory/blob/master/scripts/min_snap_traj.py)
* [Reference: Python Robotics](https://github.com/AtsushiSakai/PythonRobotics)

### Plan of Action
Given the location and heading of the target objects,
1. We split the purple objects in two clusters.
2. The respective colors + purple objects in the cluster closer to the starting location of the drones are assigned to that drone as waypoints.
3. A TSP algorithm then decides the optimal sequence of the assigned waypoints.
4. A path planning algorithm such as A* or RRT is then used to find a path joining the goal locations whilst avoiding obstacles.
5. The path is then smoothed and dynamic constraints are applied to generate feasible trajectories for the drone.
6. When travelling along the assigned trajectory and encountering the other drone, a prioritisation scheme, via addition of delays to the drone with a longer trajectory path, allows drone-to-drone collision to be avoided.

### Task Allocation
* Clustering and TSP - Duy
* Drone and Obstacle Avoidance - Max and Loick
* Dyanmics constraints - Max and Loick
* Heading Interpolation - Peter
* Trajectory Generation - Peter

### Tasks
`mrim_planner/scripts/trajectory.py`
- [x] 199: # [STUDENTS TODO, COMPULSORY] Implement heading interpolation here
- [ ] 455: # STUDENTS TODO: Sample the path parametrization 'toppra_trajectory' (instance of TOPPRA library).
- [ ] 476: # TODO: there is still some edge-case deadlock
- [ ] 526: # TODO: slow down continually instead of as fast as possible
- [ ] 615: raise NotImplementedError('[STUDENTS TODO] Collision prevention method \'delay_till_no_collisions_occur\' not finished. You have to finish it on your own.')
- [ ] 625: # [STUDENTS TODO] CHANGE BELOW

`mrim_planner/scripts/solvers/tsp_solvers.py`
- [ ] 107: # [STUDENTS TODO] Play with distance estimates in TSP and limit number of computations
- [ ] 271: raise NotImplementedError('[STUDENTS TODO] KMeans clustering of viewpoints not implemented. You have to finish it on your own')
- [ ] 276: # TODO: fill 1D list 'labels' of size len(viewpoints) with indices of the robots

`mrim_planner/scripts/path_planners/grid_based/astar.py`
- [ ] 75: raise NotImplementedError('[STUDENTS TODO] Heuristic function guiding the state space exploration not implemented. You have to finish it on your own.')
- [ ] 101: raise NotImplementedError('[STUDENTS TODO] A*: path straightening is not finished. Finish it on your own.')

`mrim_planner/scripts/path_planners/sampling_based/rrt.py`
- [ ] 142: raise NotImplementedError('[STUDENTS TODO] Implement Gaussian sampling in RRT to speed up the process and narrow the paths.')
- [ ] 233: raise NotImplementedError('[STUDENTS TODO] Getting node parents in RRT* not implemented. You have to finish it.')
- [ ] 296: raise NotImplementedError('[STUDENTS TODO] RRT: path straightening is not finished. Finish it on your own.')

`mrim_planner/config/virtual.yaml`
- [x] 7: clustering: 'random' # [STUDENTS TODO: implement better method than the default] Clustering method of viewpoints: [random (default), kmeans]
- [ ] 13: method: 'rrt' # [STUDENTS TODO: implement better method than the default] Method for generating start-to-goal paths: [euclidean, astar, rrt (default), rrtstar]
- [ ] 14: straighten_paths: false # [STUDENTS TODO: implement better method than the default] Path straightening flag (default: false)
- [ ] 20: method: 'uniform' # [STUDENTS TODO: implement better method than the default] Method for sampling random points: [uniform (default), gaussian]
- [ ] 37: with_stops: true # [STUDENTS TODO: implement better method than the default] If true, UAVs will stop at each waypoint (default: true)
- [ ] 41: method: 'none' # [STUDENTS TODO: implement better method than the default] [none (default), delay_2nd_till_1st_UAV_finishes, delay_till_no_collisions_occur]

<!---
`mrim_planner/config/real_world.yaml`
- [ ] 3: clustering: 'random' # [STUDENTS TODO: implement better method than the default] Clustering method of viewpoints: [random (default), kmeans]
- [ ] 9: method: 'rrt' # [STUDENTS TODO: implement better method than the default] Method for generating start-to-goal paths: [euclidean, astar, rrt (default), rrtstar]
- [ ] 10: straighten_paths: false # [STUDENTS TODO: implement better method than the default] Path straightening flag (default: false)
- [ ] 16: method: 'uniform' # [STUDENTS TODO: implement better method than the default] Method for sampling random points: [uniform (default), gaussian]
- [ ] 33: with_stops: true # [STUDENTS TODO: implement better method than the default] If true, UAVs will stop at each waypoint (default: true)
- [ ] 37: method: 'none' # [STUDENTS TODO: implement better method than the default] [none (default), delay_2nd_till_1st_UAV_finishes, delay_till_no_collisions_occur]
-->

`mrim_manager/scripts/manager.py`
- [ ] 73: if dist > 1.0: #TODO: add param if needed
- [ ] 665: # TODO future fix: add safety area check based on the safety area from inspection problem
- [ ] 931: rospy.Rate(10).sleep() #TODO: tune to avoid missing inspection point due to too fast flight through the inspection point