mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
578 lines
23 KiB
Python
578 lines
23 KiB
Python
# TestCableController.py
|
|
# shotmanager
|
|
#
|
|
# Unit tests for the cable controller.
|
|
#
|
|
# Created by Will Silva on 1/22/2015.
|
|
# Copyright (c) 2016 3D Robotics.
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
import cableController
|
|
from cableController import *
|
|
|
|
import multipoint
|
|
from multipoint import *
|
|
|
|
from dronekit import LocationGlobalRelative
|
|
|
|
import unittest
|
|
|
|
import mock
|
|
from mock import call
|
|
from mock import Mock
|
|
from mock import MagicMock
|
|
from mock import patch
|
|
|
|
|
|
class TestPublicInterface(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread()
|
|
self.controller.curvatureMapThread.join()
|
|
|
|
def testReachedTarget(self):
|
|
'''Tests that reachedTarget works'''
|
|
|
|
retVal = self.controller.reachedTarget()
|
|
self.assertEqual(retVal, True)
|
|
|
|
def testSetTargetP(self):
|
|
'''Test that setTargetP works'''
|
|
|
|
self.controller.setTargetP(.3)
|
|
self.assertEqual(self.controller.targetP, .3)
|
|
|
|
def testTrackSpeed(self):
|
|
'''Test that trackSpeed works'''
|
|
|
|
self.controller.trackSpeed(4)
|
|
self.assertEqual(self.controller.desiredSpeed, 4)
|
|
|
|
@mock.patch('cableController.constrain', return_value = 3)
|
|
def testUpdate(self, constrain):
|
|
'''Test that update works'''
|
|
|
|
#Mock _traverse
|
|
self.controller._traverse = Mock()
|
|
|
|
self.controller.update(UPDATE_TIME)
|
|
self.assertEqual(self.controller.desiredSpeed, 0)
|
|
self.assertEqual(self.controller.speed, 3)
|
|
self.controller._traverse.assert_called_with(UPDATE_TIME)
|
|
|
|
def testSetCurrentP(self):
|
|
'''Test that setCurrentP works'''
|
|
|
|
self.controller.setCurrentP(0.3)
|
|
self.assertEqual(self.controller.currentP, 0.3)
|
|
|
|
def testKillCurvatureMapThread(self):
|
|
'''Test that poisonPill is set to True'''
|
|
|
|
self.controller.killCurvatureMapThread()
|
|
self.assertEqual(self.controller.poisonPill, True)
|
|
|
|
class Test_computeCurvatureMap(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
self.controller.curvatureMapSpeedLimits[0] = None
|
|
|
|
def testPositiveSpeed(self):
|
|
'''Test when self.speed is positive'''
|
|
|
|
self.controller.speed = 3
|
|
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
|
|
self.controller.poisonPill = True
|
|
self.controller._computeCurvatureMap()
|
|
|
|
def testNegativeSpeed(self):
|
|
'''Test when self.speed is negative'''
|
|
|
|
self.controller.speed = -3
|
|
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
|
|
self.controller.poisonPill = True
|
|
self.controller._computeCurvatureMap()
|
|
|
|
def testZeroSpeed(self):
|
|
'''Test when self.speed is zero'''
|
|
|
|
self.controller.speed = 0
|
|
self.controller._computeCurvatureMapSpeedLimit = Mock(side_effect=[False,False,True])
|
|
self.controller.poisonPill = True
|
|
self.controller._computeCurvatureMap()
|
|
|
|
class Test_computeCurvatureMapSpeedLimit(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
self.controller.curvatureMapSpeedLimits[0] = None
|
|
|
|
def testSpeedLimitNotNone(self):
|
|
'''Test when speed limit is already computed'''
|
|
|
|
self.controller.curvatureMapSpeedLimits[0] = 3
|
|
retVal = self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(retVal, False)
|
|
|
|
def testCurrentMapSegJointIsNone(self):
|
|
'''Test when non dimensional index for the joint has not been computed yet'''
|
|
|
|
self.controller.curvatureMapJointsNonDimensional[0] = None
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
assert self.controller.curvatureMapJointsNonDimensional[0] is not None
|
|
|
|
def testCurrentMapSegJointPlusOneIsNone(self):
|
|
'''Test when non dimensional index for the joint has not been computed yet'''
|
|
|
|
self.controller.curvatureMapJointsNonDimensional[1] = None
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
assert self.controller.curvatureMapJointsNonDimensional[1] is not None
|
|
|
|
def testAltitudeLimit(self):
|
|
'''Tests that altitude limit breach sets maxAltExceeded flag'''
|
|
|
|
self.controller.posZLimit = 10 # meters
|
|
self.controller.spline.position = Mock(return_value = Vector3(0,0,-11)) # meters
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(self.controller.maxAltExceeded, True)
|
|
|
|
def testAltitudeLimitDisabled(self):
|
|
'''Tests that altitude limit breach does not set maxAltExceeded flag if altitude limit is disabled'''
|
|
|
|
self.controller.posZLimit = None
|
|
self.controller.spline.position = Mock(return_value = Vector3(0,0,-11)) # meters
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(self.controller.maxAltExceeded, False)
|
|
|
|
|
|
def testNonZeroMaxCurvature(self):
|
|
'''Test that a speed limit is calculated based on non-zero curvature'''
|
|
|
|
self.controller.spline.curvature = Mock(return_value = 3.)
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], math.sqrt(NORM_ACCEL_LIMIT/3.))
|
|
|
|
def testMinSpeedLimit(self):
|
|
'''Test that a speed limit is capped at minimum for high curvature'''
|
|
|
|
self.controller.spline.curvature = Mock(return_value = 300.)
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], MIN_CRUISE_SPEED)
|
|
|
|
def testZeroCurvature(self):
|
|
'''Test that speed limit is set to MAX_SPEED if max curvature is zero'''
|
|
|
|
self.controller.spline.curvature = Mock(return_value = 0.)
|
|
self.controller._computeCurvatureMapSpeedLimit(0)
|
|
self.assertEqual(self.controller.curvatureMapSpeedLimits[0], MAX_SPEED)
|
|
|
|
|
|
class Test_getCurvatureMapSpeedLimit(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testSegmentLessThanZero(self):
|
|
'''Test that 0. is returned if segment requested is less than zero'''
|
|
|
|
retVal = self.controller._getCurvatureMapSpeedLimit(-1)
|
|
self.assertEqual(retVal, 0.)
|
|
|
|
def testSegmentGreaterThanNumOfSegments(self):
|
|
'''Test that 0. is returned if segment requested is greater than available number of map segments'''
|
|
|
|
self.controller.curvatureMapNumSegments = 3
|
|
retVal = self.controller._getCurvatureMapSpeedLimit(4)
|
|
self.assertEqual(retVal, 0.)
|
|
|
|
class Test_traverse(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testCurrentUGreaterThanOneNotLastSegment(self):
|
|
'''Test that we advance segments if currentU is greater than 1 and it's not the last segment'''
|
|
|
|
self.controller.currentSeg = 0
|
|
self.controller.currentU = 1.0
|
|
self.controller.speed = MAX_SPEED
|
|
self.controller._traverse(UPDATE_TIME)
|
|
self.assertEqual(self.controller.currentSeg, 1)
|
|
self.assertEqual(self.controller.currentU , 0.)
|
|
|
|
def testCurrentUGreaterThanOneLastSegment(self):
|
|
'''Test that we do NOT advance segments if currentU is greater than 1 and it IS the last segment'''
|
|
|
|
self.controller.currentSeg = 2
|
|
self.controller.currentU = 1.0
|
|
self.controller.speed = MAX_SPEED
|
|
self.controller._traverse(UPDATE_TIME)
|
|
self.assertEqual(self.controller.currentSeg, 2)
|
|
self.assertEqual(self.controller.currentU , 1.)
|
|
|
|
def testCurrentULessThanZeroNotFirstSegment(self):
|
|
'''Test that we regress segments if currentU is less than 0 and it's not the first segment'''
|
|
|
|
self.controller.currentSeg = 2
|
|
self.controller.currentU = 0.0
|
|
self.controller.speed = -MAX_SPEED
|
|
self.controller._traverse(UPDATE_TIME)
|
|
self.assertEqual(self.controller.currentSeg, 1)
|
|
self.assertEqual(self.controller.currentU , 1.)
|
|
|
|
def testCurrentULessThanZeroFirstSegment(self):
|
|
'''Test that we do NOT regress segments if currentU is less than 0 and it IS the first segment'''
|
|
|
|
self.controller.currentSeg = 0
|
|
self.controller.currentU = 0.0
|
|
self.controller.speed = -MAX_SPEED
|
|
self.controller._traverse(UPDATE_TIME)
|
|
self.assertEqual(self.controller.currentSeg, 0)
|
|
self.assertEqual(self.controller.currentU , 0.)
|
|
|
|
|
|
class Test_constrainSpeed(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testMaxSpeedGreaterThanSpeedLimitGreaterThanSpeedGreaterThanZero(self):
|
|
'''Test when 0 < speed < speed limit < max speed'''
|
|
|
|
self.controller._getPosSpeedLimit = Mock(return_value = 6.)
|
|
retVal = self.controller._constrainSpeed(4.)
|
|
self.assertEqual(retVal, 4.)
|
|
|
|
def testMaxSpeedGreaterThanSpeedGreaterThanSpeedLimitGreaterThanZero(self):
|
|
'''Test when 0 < speed limit < speed < max speed'''
|
|
|
|
self.controller._getPosSpeedLimit = Mock(return_value = 3.)
|
|
retVal = self.controller._constrainSpeed(4.)
|
|
self.assertEqual(retVal, 3.)
|
|
|
|
def testSpeedGreaterThanMaxSpeedGreaterThanSpeedLimitGreaterThanZero(self):
|
|
'''Test when 0 < speed limit < max speed < speed'''
|
|
|
|
self.controller._getPosSpeedLimit = Mock(return_value = 3.)
|
|
retVal = self.controller._constrainSpeed(9.)
|
|
self.assertEqual(retVal, 3.)
|
|
|
|
def testSpeedGreaterThanSpeedLimitGreaterThanMaxSpeedGreaterThanZero(self):
|
|
'''Test when 0 < max speed < speed limit < speed'''
|
|
|
|
self.controller._getPosSpeedLimit = Mock(return_value = 10.)
|
|
retVal = self.controller._constrainSpeed(9.)
|
|
self.assertEqual(retVal, MAX_SPEED)
|
|
|
|
def testSpeedLessThanSpeedLimitLessThanMaxSpeedLessThanZero(self):
|
|
'''Test when speed < speed limit < max speed < 0'''
|
|
|
|
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
|
|
retVal = self.controller._constrainSpeed(-10.)
|
|
self.assertEqual(retVal, -MAX_SPEED)
|
|
|
|
def testSpeedLimitLessThanSpeedLessThanMaxSpeedLessThanZero(self):
|
|
'''Test when speed limit < speed < max speed < 0'''
|
|
|
|
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
|
|
retVal = self.controller._constrainSpeed(-8.5)
|
|
self.assertEqual(retVal, -MAX_SPEED)
|
|
|
|
def testSpeedLimitLessThanMaxSpeedLessThanSpeedLessThanZero(self):
|
|
'''Test when speed limit < max speed < speed < 0'''
|
|
|
|
self.controller._getNegSpeedLimit = Mock(return_value = -9.)
|
|
retVal = self.controller._constrainSpeed(-4.)
|
|
self.assertEqual(retVal, -4.)
|
|
|
|
def testMaxSpeedLessThanSpeedLessThanSpeedLimitLessThanZero(self):
|
|
'''Test when max speed < speed < speedLimit < 0'''
|
|
|
|
self.controller._getNegSpeedLimit = Mock(return_value = -4.)
|
|
retVal = self.controller._constrainSpeed(-5.)
|
|
self.assertEqual(retVal, -4.)
|
|
|
|
|
|
class Test_speedCurve(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
self.smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, self.smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testSpeedGreaterThanLinearVelocity(self):
|
|
'''Test that if speed is greater than linear_velocity then return sqrt curve speed profile'''
|
|
|
|
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
|
|
lDist = lVelocity / self.smoothStopP
|
|
speed = lVelocity + 1 #m/s
|
|
|
|
dist = 10 #meters
|
|
|
|
expectedVal = math.sqrt(2. * TANGENT_ACCEL_LIMIT * (speed**2/(2.*TANGENT_ACCEL_LIMIT) + dist))
|
|
retVal = self.controller._speedCurve(dist, speed)
|
|
self.assertEqual(retVal, expectedVal)
|
|
|
|
def testSpeedLessThanLinearVelocityAndP2GreaterThanLinearDist(self):
|
|
'''Test that if speed is less than linear_velocity and P2 is greater than linear distance then return linear speed profile'''
|
|
|
|
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
|
|
lDist = lVelocity / self.smoothStopP
|
|
speed = lVelocity - 1 #m/s
|
|
p1 = speed / self.smoothStopP
|
|
p2 = lDist + 1
|
|
dist = p2 - p1 # meters
|
|
expectedVal = math.sqrt(2. * TANGENT_ACCEL_LIMIT * (p2 - 0.5*lDist))
|
|
retVal = self.controller._speedCurve(dist, speed)
|
|
self.assertEqual(retVal, expectedVal)
|
|
|
|
def testSpeedLessThanLinearVelocityAndP2LessThanLinearDist(self):
|
|
'''Test that if speed is less than linear_velocity and P2 is less than linear distance then return linear speed profile'''
|
|
|
|
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
|
|
lDist = lVelocity / self.smoothStopP
|
|
speed = lVelocity - 1 #m/s
|
|
p1 = speed / self.smoothStopP
|
|
p2 = lDist - 1
|
|
dist = p2 - p1 # meters
|
|
expectedVal = p2 * self.smoothStopP
|
|
retVal = self.controller._speedCurve(dist, speed)
|
|
self.assertEqual(retVal, expectedVal)
|
|
|
|
|
|
class Test_maxLookAheadDist(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
self.smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, self.smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testAbsSpeedGreaterThanLinearVelocity(self):
|
|
'''Test if absolute value of speed is greater than linear velocity then return distance required to stop at constant deceleration'''
|
|
|
|
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
|
|
lDist = lVelocity / self.smoothStopP
|
|
self.controller.speed = lVelocity + 1
|
|
retVal = self.controller._maxLookAheadDist()
|
|
self.assertEqual(retVal, 0.5 * abs(self.controller.speed)**2 / TANGENT_ACCEL_LIMIT + 0.5*lDist)
|
|
|
|
def testAbsSpeedLessThanLinearVelocity(self):
|
|
'''Test if absolute value of speed is less than linear velocity then return distance required to stop at constant deceleration'''
|
|
|
|
lVelocity = TANGENT_ACCEL_LIMIT / self.smoothStopP
|
|
lDist = lVelocity / self.smoothStopP
|
|
self.controller.speed = lVelocity - 1
|
|
retVal = self.controller._maxLookAheadDist()
|
|
self.assertEqual(retVal, abs(self.controller.speed)/self.smoothStopP)
|
|
|
|
|
|
class Test_getCurvatureMapSegment(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testPIsOneHalf(self):
|
|
'''Test if P = 0.5 for 100 segment map'''
|
|
|
|
self.controller.curvatureMapNumSegments = 100 # segments
|
|
self.controller.curvatureMapSegLengthP = 1./100 # p per map segment
|
|
retVal = self.controller._getCurvatureMapSegment(0.5)
|
|
self.assertEqual(retVal,50)
|
|
|
|
|
|
class Test_getDistToCurvatureMapSegmentBegin(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testP1IsOneHalf(self):
|
|
'''Test if P1 = 0.5, P2 = 0.6, 100 meter cable'''
|
|
|
|
self.controller.spline.totalArcLength = 100 #meters
|
|
self.controller.curvatureMapJointsP[2] = 0.6 #p
|
|
retVal = self.controller._getDistToCurvatureMapSegmentBegin(0.5, 2)
|
|
self.assertAlmostEqual(retVal, 10)
|
|
|
|
|
|
class Test_getDistToCurvatureMapSegmentEnd(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testP1IsOneHalf(self):
|
|
'''Test if P1 = 0.5, P2 = 0.4, 100 meter cable'''
|
|
|
|
self.controller.spline.totalArcLength = 100 #meters
|
|
self.controller.curvatureMapJointsP[2] = 0.4 #p
|
|
retVal = self.controller._getDistToCurvatureMapSegmentBegin(0.5, 2)
|
|
self.assertAlmostEqual(retVal, 10)
|
|
|
|
|
|
class Test_getPosSpeedLimit(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testTargetPGreaterThanCurrentP(self):
|
|
'''Test if targetP is greater than currentP then speedLimit is compared with map segment speedlimit and speedCurve'''
|
|
|
|
self.controller.targetP = .5
|
|
self.controller.currentP = .3
|
|
self.controller._speedCurve = Mock(side_effect = [2.,1.])
|
|
retVal = self.controller._getPosSpeedLimit(self.controller.currentP)
|
|
self.assertEqual(retVal, 1.)
|
|
|
|
|
|
class Test_getNegSpeedLimit(unittest.TestCase):
|
|
def setUp(self):
|
|
points = [Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(2, 0, 0), Vector3(3, 0, 0)]
|
|
maxSpeed = MAX_SPEED
|
|
minSpeed = MIN_CRUISE_SPEED
|
|
tanAccelLim = TANGENT_ACCEL_LIMIT
|
|
normAccelLim = NORM_ACCEL_LIMIT
|
|
smoothStopP = 0.7
|
|
maxAlt = 50
|
|
self.controller = cableController.CableController(points, maxSpeed, minSpeed, tanAccelLim, normAccelLim, smoothStopP, maxAlt)
|
|
self.controller.killCurvatureMapThread() #send thread poison pill
|
|
self.controller.curvatureMapThread.join() #wait for thread to die
|
|
|
|
def testTargetPLessThanCurrentP(self):
|
|
'''Test if targetP is less than currentP then speedLimit is compared with map segment speedlimit and speedCurve'''
|
|
|
|
self.controller.targetP = .3
|
|
self.controller.currentP = .5
|
|
self.controller._speedCurve = Mock(side_effect = [2.,1.])
|
|
retVal = self.controller._getNegSpeedLimit(self.controller.currentP)
|
|
self.assertEqual(retVal, -1.)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|