Optimize Obstacle IsValid method (and maybe clearance as well)

[tylerlum]

The speed of Obstacle IsValid and clearance methods makes a big impact on the pathfinding performance and speed. From testing, it seems that wedge is always much faster than ellipse and ellipse is much faster than circle.

In summary for checking invalid:
Wedge takes about 8.02675882975e-07 seconds per ship
Ellipse takes about 7.77244567871e-06 seconds per ship
Circles takes about 7.49667485555e-05 seconds per ship (though per circle was 1.22414300722e-06)

About a factor of Circles is 10x slower than Ellipse. Ellipse is 10x slower than Wedge.

I think that we should try to optimize these invalid methods.
ideas: https://stackoverflow.com/questions/16210563/why-is-computing-point-distances-so-slow-in-python

Initial Test Code

#!/usr/bin/env python
import rospy
import time
from Sailbot import *
from utilities import *
from local_pathfinding.msg import latlon, AISShip, AISMsg

if __name__ == "__main__":
    # Create sailbot ROS object that subscribes to relevant topics
    sailbot = Sailbot(nodeName='testing')
    referenceLatlon = PORT_RENFREW_LATLON
    obstacleTypesByIndex = ['ellipse', 'wedge', 'circles']
    obstacleTypeIndex = 0

    while not rospy.is_shutdown():
        state = sailbot.getCurrentState()
        if len(state.AISData.ships) == 0:
            continue

        obstacleType = obstacleTypesByIndex[obstacleTypeIndex]
        obstacles = getObstacles(state.AISData.ships, state.position, state.speedKmph, referenceLatlon, obstacleType)

        startTime = time.time()
        xy = [0, 0]
        isValidPosition = isValid(xy, obstacles)
        endTime = time.time()

        rospy.logwarn("{} took {} seconds to run isValid on {} ships".format(obstacleType, endTime - startTime, len(state.AISData.ships)))
        rospy.logwarn("This means on average {} seconds per ship.".format((endTime - startTime) / len(state.AISData.ships)))
        if obstacleType == 'circles':
            numObstacles = 0
            for c in obstacles:
                numObstacles += c.numObstacles()
            rospy.logwarn("Actually made of {} circles".format(numObstacles))
            rospy.logwarn("This means on average {} seconds per circle.".format((endTime - startTime) / numObstacles))

        time.sleep(1)
        # Increment obstacleTypeIndex
        obstacleTypeIndex = (obstacleTypeIndex + 1) % len(obstacleTypesByIndex)

        if obstacleTypeIndex == 0:
            rospy.logwarn("***************************")

Output

[WARN] [1586645751.743330]: ellipse took 0.000300168991089 seconds to run isValid on 30 ships
[WARN] [1586645751.743982]: This means on average 1.00056330363e-05 seconds per ship.
[WARN] [1586645752.756646]: wedge took 2.38418579102e-05 seconds to run isValid on 30 ships
[WARN] [1586645752.757281]: This means on average 7.94728597005e-07 seconds per ship.
[WARN] [1586645753.780007]: circles took 0.00224900245667 seconds to run isValid on 30 ships
[WARN] [1586645753.781411]: This means on average 7.49667485555e-05 seconds per ship.
[WARN] [1586645753.782206]: Actually made of 1949 circles
[WARN] [1586645753.782943]: This means on average 1.15392635026e-06 seconds per circle.
[WARN] [1586645754.784941]: ***************************
[WARN] [1586645754.802615]: ellipse took 0.000233173370361 seconds to run isValid on 30 ships
[WARN] [1586645754.803412]: This means on average 7.77244567871e-06 seconds per ship.
[WARN] [1586645755.819788]: wedge took 2.40802764893e-05 seconds to run isValid on 30 ships
[WARN] [1586645755.820447]: This means on average 8.02675882975e-07 seconds per ship.
[WARN] [1586645756.837780]: circles took 0.00169205665588 seconds to run isValid on 30 ships
[WARN] [1586645756.838484]: This means on average 5.64018885295e-05 seconds per ship.
[WARN] [1586645756.839100]: Actually made of 1949 circles
[WARN] [1586645756.839791]: This means on average 8.6816657562e-07 seconds per circle.
[WARN] [1586645757.841762]: ***************************
[WARN] [1586645757.859857]: ellipse took 0.000216007232666 seconds to run isValid on 30 ships
[WARN] [1586645757.860469]: This means on average 7.20024108887e-06 seconds per ship.
[WARN] [1586645758.904467]: wedge took 4.91142272949e-05 seconds to run isValid on 30 ships
[WARN] [1586645758.905599]: This means on average 1.63714090983e-06 seconds per ship.
[WARN] [1586645759.931512]: circles took 0.00238585472107 seconds to run isValid on 30 ships
[WARN] [1586645759.932177]: This means on average 7.95284907023e-05 seconds per ship.
[WARN] [1586645759.932842]: Actually made of 1949 circles
[WARN] [1586645759.933481]: This means on average 1.22414300722e-06 seconds per circle.

[tylerlum]

Stored the above testing code in branch: OptimizeObstacles.