shotmanager/Test/TestVectorPathHandler.py at master · OpenSolo/shotmanager · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
# Unit tests for ROI
import mock
from mock import Mock
import os
from os import sys, path
from pymavlink import mavutil
from shotManagerConstants import *
import unittest
import random
import math

from dronekit import Vehicle, LocationGlobalRelative

sys.path.append(os.path.realpath('..'))
import pathHandler
from shotManager import ShotManager
import shots
import vectorPathHandler

#Random number generator seed
SEED = 94739473

#Number of tests to run
REPEAT = 10

class TestMove(unittest.TestCase):
    def setUp(self):
        mgr = mock.create_autospec(ShotManager)
        #mgr.currentShot = shots.APP_SHOT_ZIPLINE
        v = mock.create_autospec(Vehicle)
        v.message_factory = Mock()
        v.commands = Mock()
        self.handler = vectorPathHandler.VectorPathHandler(v, mgr, 0, 0)
        random.seed(SEED)

    def testTogglePauseShouldPause(self):
        '''If cruiseSpeed != 0 then pause() should be executed'''
        self.handler.pause = Mock()
        self.handler.cruiseSpeed = 4.0
        self.handler.togglePause()
        self.handler.pause.assert_called_with()

    def testTogglePauseShouldResume(self):
        '''If cruiseSpeed == 0 then resume() should be executed'''
        self.handler.resume = Mock()
        self.handler.cruiseSpeed = 0.0
        self.handler.togglePause()
        self.handler.resume.assert_called_with()

    def testSpotLock(self):
        ''' test accurcy of spot lock'''
        vect = self.handler.getUnitVectorFromHeadingAndTilt(0, -45)
        self.assertEqual(int(vect.y*1000), 0)
        self.assertEqual(int(vect.x*1000), 707)
        self.assertEqual(int(vect.z*1000), -707)
        vect = self.handler.getUnitVectorFromHeadingAndTilt(90, -15)
        self.assertEqual(int(vect.y*1000), 965)
        self.assertEqual(int(vect.x*1000), 0)
        self.assertEqual(int(vect.z*1000), -258)
        vect = self.handler.getUnitVectorFromHeadingAndTilt(180, 0)
        self.assertEqual(int(vect.y*1000), 0)
        self.assertEqual(int(vect.x*1000), -1000)
        self.assertEqual(int(vect.z*1000), 0)

    def testNoCruiseForwardsFullStick(self):
        """ Command should reach (and not exceed) MAX_SPEED [Flying in forwards (+) direction, full stick in positive direction] """
        for test in range(REPEAT): #test REPEAT times with random values based on SEED
            self.handler.cruiseSpeed = 0.0 #No cruise
            initialSpeed = random.uniform(0.0,vectorPathHandler.MAX_PATH_SPEED) #Start at a random positive speed within acceptable limits
            self.handler.accel = 0.136000
            self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
            channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Full positive stick
            channels[ROLL] = 1.0
            #calculate number of ticks required to reach MAX_SPEED
            deltaSpeed = vectorPathHandler.MAX_PATH_SPEED - self.handler.currentSpeed
            ticksRequired = int(math.ceil(deltaSpeed/self.handler.accel))

            for ticks in range(1,ticksRequired+1):
                #run controller tick
                returnSpeed = self.handler.move( channels )
                #make sure we are targeted to the right loc
            self.assertEqual(returnSpeed, vectorPathHandler.MAX_PATH_SPEED)

    def testNoCruiseBackwardsFullNegStick(self):
        """ Command should reach (and not exceed) MAX_SPEED [Flying in rewards (-) direction, full stick in negative direction] """
        for test in range(REPEAT): #test REPEAT times with random values based on SEED
            self.handler.cruiseSpeed = 0.0 #No cruise
            self.handler.accel = 0.136000
            initialSpeed = 0
            self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
            channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Full negative stick
            channels[ROLL] = -1.0
            self.handler.accel = 1.0
            #calculate number of ticks required to reach MAX_SPEED
            deltaSpeed = -vectorPathHandler.MAX_PATH_SPEED - self.handler.currentSpeed
            ticksRequired = int(math.ceil(abs(deltaSpeed/self.handler.accel)))

            for ticks in range(1,ticksRequired+1):
                #run controller tick
                returnSpeed = self. handler.move( channels )
                #make sure we are targeted to the right loc
                self.assertGreaterEqual(returnSpeed, 0.0)
            self.assertEqual(returnSpeed, vectorPathHandler.MAX_PATH_SPEED)

# THROTTLE = 0
# ROLL = 1
# PITCH = 2
# YAW = 3
# FILTERED_PADDLE = 5
# RAW_PADDLE = 7

    def testCruiseSameDir(self):
        """ Cruising with full same stick towards loc 2- should accelerate """
        self.handler.cruiseSpeed = 4.0 #random positive cruise speed
        self.handler.accel = 0.136000
        self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
        channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Stick forward
        channels[PITCH] = -1.0

        returnSpeed = self.handler.move( channels )
        self.assertGreaterEqual(returnSpeed, self.handler.cruiseSpeed)


    def testCruiseOppositeDir(self):
        """ Cruising towards loc 2 with opposite stick - should decelerate to a steady state speed"""
        self.handler.cruiseSpeed = 4.0 #random positive cruise speed
        self.handler.accel = 0.136000
        self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
        channels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Stick backward
        channels[PITCH] = 1.0
        returnSpeed = self.handler.move( channels )
        self.assertLessEqual(returnSpeed, self.handler.cruiseSpeed)